From a2f0183297284ed43ef5d435b4e11b414ec21876 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 15 Aug 2023 21:17:52 +0200 Subject: [PATCH] Remove added path index from planner adapter function signature (#2285) --- .../planning_interface/planning_response.h | 5 +++ .../planning_request_adapter.h | 30 +++----------- .../src/planning_request_adapter.cpp | 41 ++++--------------- .../src/chomp_optimizer_adapter.cpp | 6 +-- .../src/stomp_moveit_smoothing_adapter.cpp | 4 +- .../planning_pipeline/planning_pipeline.h | 12 ------ .../src/planning_pipeline.cpp | 39 +++++++----------- .../src/add_ruckig_traj_smoothing.cpp | 4 +- .../src/add_time_optimal_parameterization.cpp | 4 +- .../src/empty.cpp | 4 +- .../src/fix_start_state_bounds.cpp | 10 +++-- .../src/fix_start_state_collision.cpp | 10 +++-- .../src/fix_start_state_path_constraints.cpp | 16 +++++--- .../src/fix_workspace_bounds.cpp | 4 +- .../src/resolve_constraint_frames.cpp | 4 +- 15 files changed, 72 insertions(+), 121 deletions(-) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 7f8bbadcb6..3b4db78f08 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -65,6 +65,11 @@ struct MotionPlanResponse moveit_msgs::msg::RobotState start_state; std::string planner_id; + /// Sometimes planning request adapters may add states on the solution path (e.g., add the current state of the robot + /// as prefix, when the robot started to plan only from near that state, as the current state itself appears to touch + /// obstacles). This is helpful because the added states should not be considered invalid in all situations. + std::vector added_path_index; // This won't be included into the MotionPlanResponse ROS 2 message! + // \brief Enable checking of query success or failure, for example if(response) ... explicit operator bool() const { diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index 1d2d8e762a..4c0a346747 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -46,10 +46,6 @@ /** \brief Generic interface to adapting motion planning requests */ namespace planning_request_adapter { -namespace -{ -std::vector empty_path_index_vector = {}; -} MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc /** @brief Concept in MoveIt which can be used to modify the planning problem and resulting trajectory (pre-processing @@ -84,16 +80,11 @@ class PlanningRequestAdapter * @param planning_scene Representation of the environment for the planning * @param req Motion planning request with a set of constraints * @param res Reference to which the generated motion plan response is written to - * @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be considered - invalid in all situations. * @return True if response got generated correctly */ [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index = empty_path_index_vector) const; + planning_interface::MotionPlanResponse& res) const; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if @@ -104,15 +95,11 @@ class PlanningRequestAdapter * @param planning_scene Representation of the environment for the planning * @param req Motion planning request with a set of constraints * @param res Reference to which the generated motion plan response is written to - * @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be - considered invalid in all situations. * @return True if response got generated correctly */ - [[nodiscard]] virtual bool - adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index = empty_path_index_vector) const = 0; + [[nodiscard]] virtual bool adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const = 0; }; /** \brief Apply a sequence of adapters to a motion plan */ @@ -128,16 +115,11 @@ class PlanningRequestAdapterChain * @param planning_scene Representation of the environment for the planning * @param req Motion planning request with a set of constraints * @param res Reference to which the generated motion plan response is written to - * @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be - considered invalid in all situations. * @return True if response got generated correctly */ [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index = empty_path_index_vector) const; + planning_interface::MotionPlanResponse& res) const; private: std::vector adapters_; diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index 0a7a7bddfc..8f4c134996 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -64,12 +64,11 @@ bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAdapter::PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) { try { - bool result = adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index); + bool result = adapter.adaptAndPlan(planner, planning_scene, req, res); RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code)); return result; } @@ -77,7 +76,6 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda { RCLCPP_ERROR(LOGGER, "Exception caught executing adapter '%s': %s\nSkipping adapter instead.", adapter.getDescription().c_str(), ex.what()); - added_path_index.clear(); return planner(planning_scene, req, res); } } @@ -87,15 +85,14 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const + planning_interface::MotionPlanResponse& res) const { return adaptAndPlan( [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) { return callPlannerInterfaceSolve(*planner, scene, req, res); }, - planning_scene, req, res, added_path_index); + planning_scene, req, res); } void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter) @@ -106,17 +103,13 @@ void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPt bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const + planning_interface::MotionPlanResponse& res) const { // if there are no adapters, run the planner directly if (adapters_.empty()) { - added_path_index.clear(); return callPlannerInterfaceSolve(*planner, planning_scene, req, res); } - // the index values added by each adapter - std::vector> added_path_index_each(adapters_.size()); // if there are adapters, construct a function for each, in order, // so that in the end we have a nested sequence of functions that calls all adapters @@ -129,30 +122,14 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner for (int i = adapters_.size() - 1; i >= 0; --i) { - fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]]( - const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) { - return callAdapter(adapter, fn, scene, req, res, added_path_index); - }; + fn = [&adapter = *adapters_[i], + fn](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) { return callAdapter(adapter, fn, scene, req, res); }; } bool result = fn(planning_scene, req, res); - added_path_index.clear(); - // merge the index values from each adapter - for (std::vector& added_states_by_each_adapter : added_path_index_each) - { - for (std::size_t& added_index : added_states_by_each_adapter) - { - for (std::size_t& index_in_path : added_path_index) - { - if (added_index <= index_in_path) - index_in_path++; - } - added_path_index.push_back(added_index); - } - } - std::sort(added_path_index.begin(), added_path_index.end()); + std::sort(res.added_path_index.begin(), res.added_path_index.end()); return result; } diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index a21402dafb..e8f8b8cbe6 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -53,7 +53,7 @@ namespace chomp { -static rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner"); class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter { @@ -173,8 +173,8 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ..."); diff --git a/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp b/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp index 4f6a912fc4..c09d35635b 100644 --- a/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp @@ -71,8 +71,8 @@ class StompSmoothingAdapter : public planning_request_adapter::PlanningRequestAd } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { // Following call to planner() calls the motion planner defined for the pipeline and stores the trajectory inside // the MotionPlanResponse res variable which is then passed to STOMP for optimization diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 89ee21acb4..e45d6f9cc4 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -136,18 +136,6 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const; - /** \brief Call the motion planner plugin and the sequence of planning request adapters (if any). - \param planning_scene The planning scene where motion planning is to be done - \param req The request for motion planning - \param res The motion planning response - \param adapter_added_state_index Sometimes planning request adapters may add states on the solution path (e.g., - add the current state of the robot as prefix, when the robot started to plan only from near that state, as the - current state itself appears to touch obstacles). This is helpful because the added states should not be considered - invalid in all situations. */ - bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& adapter_added_state_index) const; - /** \brief Request termination, if a generatePlan() function is currently computing plans */ void terminate() const; diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index bd0413c9cb..0b3e4288f9 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -151,7 +151,7 @@ void planning_pipeline::PlanningPipeline::configure() // load the planner request adapters if (!adapter_plugin_names_.empty()) { - std::vector ads; + std::vector planning_request_adapter_vector; try { adapter_plugin_loader_ = @@ -168,10 +168,10 @@ void planning_pipeline::PlanningPipeline::configure() { for (const std::string& adapter_plugin_name : adapter_plugin_names_) { - planning_request_adapter::PlanningRequestAdapterPtr ad; + planning_request_adapter::PlanningRequestAdapterPtr planning_request_adapter; try { - ad = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name); + planning_request_adapter = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name); } catch (pluginlib::PluginlibException& ex) { @@ -179,20 +179,21 @@ void planning_pipeline::PlanningPipeline::configure() ex.what()); throw; } - if (ad) + if (planning_request_adapter) { - ad->initialize(node_, parameter_namespace_); - ads.push_back(std::move(ad)); + planning_request_adapter->initialize(node_, parameter_namespace_); + planning_request_adapter_vector.push_back(std::move(planning_request_adapter)); } } } - if (!ads.empty()) + if (!planning_request_adapter_vector.empty()) { adapter_chain_ = std::make_unique(); - for (planning_request_adapter::PlanningRequestAdapterConstPtr& ad : ads) + for (planning_request_adapter::PlanningRequestAdapterConstPtr& planning_request_adapter : + planning_request_adapter_vector) { - RCLCPP_INFO(LOGGER, "Using planning request adapter '%s'", ad->getDescription().c_str()); - adapter_chain_->addAdapter(ad); + RCLCPP_INFO(LOGGER, "Using planning request adapter '%s'", planning_request_adapter->getDescription().c_str()); + adapter_chain_->addAdapter(planning_request_adapter); } } } @@ -247,15 +248,6 @@ void planning_pipeline::PlanningPipeline::checkSolutionPaths(bool flag) bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const -{ - std::vector dummy; - return generatePlan(planning_scene, req, res, dummy); -} - -bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& adapter_added_state_index) const { // Set planning pipeline active active_ = true; @@ -265,7 +257,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla { received_request_publisher_->publish(req); } - adapter_added_state_index.clear(); if (!planner_instance_) { @@ -281,11 +272,11 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla { if (adapter_chain_) { - solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index); - if (!adapter_added_state_index.empty()) + solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res); + if (!res.added_path_index.empty()) { std::stringstream ss; - for (std::size_t added_index : adapter_added_state_index) + for (std::size_t added_index : res.added_path_index) ss << added_index << ' '; RCLCPP_INFO(LOGGER, "Planning adapters have added states at index positions: [ %s]", ss.str().c_str()); } @@ -327,7 +318,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla for (std::size_t i = 0; i < index.size() && !problem; ++i) { bool found = false; - for (std::size_t added_index : adapter_added_state_index) + for (std::size_t added_index : res.added_path_index) { if (index[i] == added_index) { diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index 950ade9c60..2e3638397b 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -62,8 +62,8 @@ class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRe } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 3e2dec6593..677494212c 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -62,8 +62,8 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp index de08753980..577a995962 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp @@ -51,8 +51,8 @@ class Empty : public planning_request_adapter::PlanningRequestAdapter } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { return planner(planning_scene, req, res); } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index c68dd6bb04..ae755e8320 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -75,8 +75,8 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -197,9 +197,11 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap res.trajectory->getAverageSegmentDuration())); res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions - for (std::size_t& added_index : added_path_index) + for (std::size_t& added_index : res.added_path_index) + { added_index++; - added_path_index.push_back(0); + } + res.added_path_index.push_back(0); } return solved; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index 7e8d3a361c..b58196d059 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -72,8 +72,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -146,9 +146,11 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA res.trajectory->getAverageSegmentDuration())); res.trajectory->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions - for (std::size_t& added_index : added_path_index) + for (std::size_t& added_index : res.added_path_index) + { added_index++; - added_path_index.push_back(0); + } + res.added_path_index.push_back(0); } return solved; } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index d628ef9bf4..3453dd0ec1 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -69,8 +69,8 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -94,9 +94,9 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe // we call the planner for this additional request, but we do not want to include potential // index information from that call std::vector added_path_index_temp; - added_path_index_temp.swap(added_path_index); + added_path_index_temp.swap(res.added_path_index); bool solved1 = planner(planning_scene, req2, res2); - added_path_index_temp.swap(added_path_index); + added_path_index_temp.swap(res.added_path_index); if (solved1) { @@ -111,12 +111,16 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe if (solved2) { // since we add a prefix, we need to correct any existing index positions - for (std::size_t& added_index : added_path_index) + for (std::size_t& added_index : res.added_path_index) + { added_index += res2.trajectory->getWayPointCount(); + } // we mark the fact we insert a prefix path (we specify the index position we just added) for (std::size_t i = 0; i < res2.trajectory->getWayPointCount(); ++i) - added_path_index.push_back(i); + { + res.added_path_index.push_back(i); + } // we need to append the solution paths. res2.trajectory->append(*res.trajectory, 0.0); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp index 35fbbb6134..72c1bd924d 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp @@ -66,8 +66,8 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); const moveit_msgs::msg::WorkspaceParameters& wparams = req.workspace_parameters; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index a259d4404c..212c69db8a 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -55,8 +55,8 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest } bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); planning_interface::MotionPlanRequest modified = req;