From 604b36b12e6b8fb1bd12aebe5c58e75523620945 Mon Sep 17 00:00:00 2001 From: Shobuj Paul Date: Sat, 2 Mar 2024 22:52:19 +0530 Subject: [PATCH] Stored trajectories for multiple iterations in res.trajectories vector --- .../src/stomp_moveit_planning_context.cpp | 125 ++++++++++-------- 1 file changed, 68 insertions(+), 57 deletions(-) diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index d61fcbcf64d..bcb7ab72054 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -279,72 +279,83 @@ void StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) void StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { - // Start time - auto time_start = std::chrono::steady_clock::now(); - - res.planner_id = std::string("stomp"); - // Default to happy path - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - - // Extract start and goal states - const auto& req = getMotionPlanRequest(); - const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); - moveit::core::RobotState goal_state(start_state); - constraint_samplers::ConstraintSamplerManager sampler_manager; - auto goal_sampler = sampler_manager.selectSampler(getPlanningScene(), getGroupName(), req.goal_constraints.at(0)); - if (!goal_sampler || !goal_sampler->sample(goal_state)) - { - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return; // Can't plan without valid goal state - } - // STOMP config, task, planner instance const auto group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); auto config = getStompConfig(params_, group->getActiveJointModels().size() /* num_dimensions */); - robot_trajectory::RobotTrajectoryPtr input_trajectory; - if (extractSeedTrajectory(request_, getPlanningScene()->getRobotModel(), input_trajectory)) + + // Reset num_iterations in config to go through and store each trajectory at each iteration + int num_iterations = config.num_iterations; + config.num_iterations = 0; + res.trajectory.resize(num_iterations); + res.description.resize(num_iterations); + res.processing_time.resize(num_iterations); + + for (int i = 0; i < num_iterations; i++) { - config.num_timesteps = input_trajectory->size(); - } - const auto task = createStompTask(config, *this); - stomp_ = std::make_shared(config, task); + // Increase number of iterations for each successive trajectory + config.num_iterations++; + + // Start time + auto time_start = std::chrono::steady_clock::now(); + + res.planner_id = std::string("stomp"); + // Default to happy path + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + + // Extract start and goal states + const auto& req = getMotionPlanRequest(); + const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); + moveit::core::RobotState goal_state(start_state); + constraint_samplers::ConstraintSamplerManager sampler_manager; + auto goal_sampler = sampler_manager.selectSampler(getPlanningScene(), getGroupName(), req.goal_constraints.at(0)); + if (!goal_sampler || !goal_sampler->sample(goal_state)) + { + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return; // Can't plan without valid goal state + } - std::condition_variable cv; - std::mutex cv_mutex; - bool finished = false; - auto timeout_future = std::async(std::launch::async, [&, stomp = stomp_]() { - std::unique_lock lock(cv_mutex); - cv.wait_for(lock, std::chrono::duration(req.allowed_planning_time), [&finished] { return finished; }); - if (!finished) + robot_trajectory::RobotTrajectoryPtr input_trajectory; + if (extractSeedTrajectory(request_, getPlanningScene()->getRobotModel(), input_trajectory)) { - stomp->cancel(); + config.num_timesteps = input_trajectory->size(); } - }); + const auto task = createStompTask(config, *this); + stomp_ = std::make_shared(config, task); + + std::condition_variable cv; + std::mutex cv_mutex; + bool finished = false; + auto timeout_future = std::async(std::launch::async, [&, stomp = stomp_]() { + std::unique_lock lock(cv_mutex); + cv.wait_for(lock, std::chrono::duration(req.allowed_planning_time), [&finished] { return finished; }); + if (!finished) + { + stomp->cancel(); + } + }); - // Solve - res.trajectory.resize(1); - if (!solveWithStomp(stomp_, start_state, goal_state, group, input_trajectory, res.trajectory[0])) - { - // We timed out if the timeout task has completed so that the timeout future is valid and ready - bool timed_out = - timeout_future.valid() && timeout_future.wait_for(std::chrono::nanoseconds(1)) == std::future_status::ready; - res.error_code.val = - timed_out ? moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT : moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - } - stomp_.reset(); - { - std::unique_lock lock(cv_mutex); - finished = true; - cv.notify_all(); - } - // Description - res.description.resize(1); - res.description[0] = "plan"; + // Solve + if (!solveWithStomp(stomp_, start_state, goal_state, group, input_trajectory, res.trajectory[i])) + { + // We timed out if the timeout task has completed so that the timeout future is valid and ready + bool timed_out = + timeout_future.valid() && timeout_future.wait_for(std::chrono::nanoseconds(1)) == std::future_status::ready; + res.error_code.val = timed_out ? moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT : + moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + } + stomp_.reset(); + { + std::unique_lock lock(cv_mutex); + finished = true; + cv.notify_all(); + } + // Description + res.description[i] = "plan_" + std::to_string(i); - // Stop time - res.processing_time.resize(1); - std::chrono::duration elapsed_seconds = std::chrono::steady_clock::now() - time_start; - res.processing_time[0] = elapsed_seconds.count(); + // Stop time + std::chrono::duration elapsed_seconds = std::chrono::steady_clock::now() - time_start; + res.processing_time[i] = elapsed_seconds.count(); + } } bool StompPlanningContext::terminate()