Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Remove added path index from function signatures #2285

Merged
merged 6 commits into from
Aug 15, 2023
Merged

Remove added path index from function signatures #2285

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
5bb1fd4
Move added path index into MotionPlanResponse datastructure
sjahr Aug 2, 2023
dd102c1
ad -> planning_request_adapter and ads -> planning_request_adapter_ve…
sjahr Aug 4, 2023
276aea9
Remove adapter_added_state_index
sjahr Aug 4, 2023
c187fe6
Merge branch 'main' into pr-remove_added_path_index_argument
sjahr Aug 4, 2023
aa41603
Fix clang-tidy
sjahr Aug 5, 2023
12e0f4d
Merge branch 'main' into pr-remove_added_path_index_argument
sjahr Aug 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::size_t> 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
{
Expand Down
30 changes: 6 additions & 24 deletions ...anning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +46,6 @@
/** \brief Generic interface to adapting motion planning requests */
namespace planning_request_adapter
{
namespace
{
std::vector<std::size_t> 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
Expand Down Expand Up @@ -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<std::size_t>& 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
Expand All @@ -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<std::size_t>& 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 */
Expand All @@ -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<std::size_t>& added_path_index = empty_path_index_vector) const;
planning_interface::MotionPlanResponse& res) const;

private:
std::vector<PlanningRequestAdapterConstPtr> adapters_;
Expand Down
41 changes: 9 additions & 32 deletions moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,20 +64,18 @@ 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<std::size_t>& 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;
}
catch (std::exception& ex)
{
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);
}
}
Expand All @@ -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<std::size_t>& 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)
Expand All @@ -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<std::size_t>& 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<std::vector<std::size_t>> 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
Expand All @@ -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<std::size_t>& 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;
}

Expand Down
6 changes: 3 additions & 3 deletions moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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<std::size_t>& /*added_path_index*/) const override
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const override
{
RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ...");

Expand Down
4 changes: 2 additions & 2 deletions moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::size_t>& /*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
Expand Down
12 changes: 0 additions & 12 deletions moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::size_t>& adapter_added_state_index) const;

/** \brief Request termination, if a generatePlan() function is currently computing plans */
void terminate() const;

Expand Down
39 changes: 15 additions & 24 deletions moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ void planning_pipeline::PlanningPipeline::configure()
// load the planner request adapters
if (!adapter_plugin_names_.empty())
{
std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> planning_request_adapter_vector;
try
{
adapter_plugin_loader_ =
Expand All @@ -168,31 +168,32 @@ 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)
{
RCLCPP_FATAL(LOGGER, "Exception while loading planning adapter plugin '%s': %s", adapter_plugin_name.c_str(),
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<planning_request_adapter::PlanningRequestAdapterChain>();
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);
}
}
}
Expand Down Expand Up @@ -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<std::size_t> 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<std::size_t>& adapter_added_state_index) const
{
// Set planning pipeline active
active_ = true;
Expand All @@ -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_)
{
Expand All @@ -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());
}
Expand Down Expand Up @@ -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)
{
Expand Down
4 changes: 2 additions & 2 deletions moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::size_t>& /*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)
Expand Down
4 changes: 2 additions & 2 deletions ...t_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::size_t>& /*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)
Expand Down
4 changes: 2 additions & 2 deletions moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::size_t>& /*added_path_index*/) const override
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const override
{
return planner(planning_scene, req, res);
}
Expand Down
Loading
Loading