Skip to content

Commit

Permalink
Planning request adapters: short-circuit if failure, return code rath…
Browse files Browse the repository at this point in the history 
…er than bool (#1605)

* Return code rather than bool

* Remove all debug prints

* Small fixup

* Minor cleanup of comment and error handling

* void return from PlannerFn

* Control reaches end of non-void function

* Use a MoveItErrorCode cast

* More efficient callAdapter()

* More MoveItErrorCode

* CI fixup attempt
  • Loading branch information
AndyZe committed Oct 31, 2022
1 parent 749c5ae commit 66a64b4
Show file tree
Hide file tree
Showing 15 changed files with 183 additions and 136 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/ci.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ jobs:
AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src
# Uninstall binaries that are duplicated in the .repos file
# TODO(andyz): remove this once a sync containing 35b93c8 happens
AFTER_SETUP_UPSTREAM_WORKSPACE_EMBED: sudo apt remove ros-${{ matrix.env.ROS_DISTRO }}-moveit-msgs -y
AFTER_SETUP_UPSTREAM_WORKSPACE_EMBED: sudo apt remove -y ros-${{ matrix.env.ROS_DISTRO }}-moveit-msgs || true
AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src
# Clear the ccache stats before and log the stats after the build
AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G
Expand Down
50 changes: 27 additions & 23 deletions ...anning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/utils/moveit_error_code.h>
#include <functional>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
Expand All @@ -51,9 +52,9 @@ MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapter
class PlanningRequestAdapter
{
public:
using PlannerFn =
std::function<bool(const planning_scene::PlanningSceneConstPtr&, const planning_interface::MotionPlanRequest&,
planning_interface::MotionPlanResponse&)>;
using PlannerFn = std::function<moveit::core::MoveItErrorCode(const planning_scene::PlanningSceneConstPtr&,
const planning_interface::MotionPlanRequest&,
planning_interface::MotionPlanResponse&)>;

PlanningRequestAdapter()
{
Expand All @@ -73,25 +74,27 @@ class PlanningRequestAdapter
return "";
}

bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const;
moveit::core::MoveItErrorCode adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const;

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) const;
moveit::core::MoveItErrorCode 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;

/** \brief Adapt the planning request if needed, call the planner
function \e planner and update the planning response if
needed. If the response is changed, the index values of the
states added without planning are added to \e
added_path_index */
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) const = 0;
virtual moveit::core::MoveItErrorCode 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 = 0;

protected:
/** \brief Helper param for getting a parameter using a namespace **/
Expand Down Expand Up @@ -128,15 +131,16 @@ class PlanningRequestAdapterChain
adapters_.push_back(adapter);
}

bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const;
moveit::core::MoveItErrorCode adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const;

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) const;
moveit::core::MoveItErrorCode 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;

private:
std::vector<PlanningRequestAdapterConstPtr> adapters_;
Expand Down
67 changes: 37 additions & 30 deletions moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,22 +45,30 @@ rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("planning_request

namespace
{
bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res)
moveit::core::MoveItErrorCode callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res)
{
planning_interface::PlanningContextPtr context = planner.getPlanningContext(planning_scene, req, res.error_code_);
if (context)
return context->solve(res);
{
// TODO(andyz): consider returning a moveit::core::MoveItErrorCode from context->solve()
bool result = context->solve(res);
return result ? moveit::core::MoveItErrorCode::SUCCESS : moveit::core::MoveItErrorCode::FAILURE;
}
else
return false;
{
return moveit::core::MoveItErrorCode::FAILURE;
}
}

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)
moveit::core::MoveItErrorCode 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)
{
try
{
Expand All @@ -77,11 +85,10 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda

} // namespace

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
moveit::core::MoveItErrorCode 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
{
return adaptAndPlan(
[&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
Expand All @@ -91,29 +98,26 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag
planning_scene, req, res, added_path_index);
}

bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const
moveit::core::MoveItErrorCode PlanningRequestAdapter::adaptAndPlan(
const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const
{
std::vector<std::size_t> dummy;
return adaptAndPlan(planner, planning_scene, req, res, dummy);
}

bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const
moveit::core::MoveItErrorCode PlanningRequestAdapterChain::adaptAndPlan(
const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const
{
std::vector<std::size_t> dummy;
return adaptAndPlan(planner, planning_scene, req, res, dummy);
}

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
moveit::core::MoveItErrorCode 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
{
// if there are no adapters, run the planner directly
if (adapters_.empty())
Expand All @@ -140,24 +144,27 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
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) {
// Abort pipeline and return in case of failure
return callAdapter(adapter, fn, scene, req, res, added_path_index);
};
}

bool result = fn(planning_scene, req, res);
moveit::core::MoveItErrorCode moveit_code = 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());
return result;
return moveit_code;
}
}

Expand Down
21 changes: 15 additions & 6 deletions moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,16 +172,20 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
return "CHOMP Optimizer";
}

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
moveit::core::MoveItErrorCode 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
{
RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ...");

// following call to planner() calls the OMPL planner and stores the trajectory inside the MotionPlanResponse res
// variable which is then used by CHOMP for optimization of the computed trajectory
if (!planner(ps, req, res))
return false;
moveit::core::MoveItErrorCode moveit_code = planner(ps, req, res);
if (!bool(moveit_code))
{
return moveit_code;
}

// create a hybrid collision detector to set the collision checker as hybrid
collision_detection::CollisionDetectorAllocatorPtr hybrid_cd(
Expand All @@ -202,10 +206,15 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
{
res.trajectory_ = res_detailed.trajectory_[0];
res.planning_time_ += res_detailed.processing_time_[0];
moveit_code = moveit::core::MoveItErrorCode::FAILURE;
}
else
{
moveit_code = moveit::core::MoveItErrorCode::SUCCESS;
}
res.error_code_ = res_detailed.error_code_;

return planning_success;
return moveit_code;
}

private:
Expand Down
3 changes: 2 additions & 1 deletion moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,7 +258,8 @@ 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);
solved =
bool(adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index));
if (!adapter_added_state_index.empty())
{
std::stringstream ss;
Expand Down
16 changes: 9 additions & 7 deletions ...s/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,23 +59,25 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan
return "Add Time Parameterization";
}

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
moveit::core::MoveItErrorCode 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
{
bool result = planner(planning_scene, req, res);
if (result && res.trajectory_)
moveit::core::MoveItErrorCode moveit_code = planner(planning_scene, req, res);
if (bool(moveit_code) && res.trajectory_)
{
RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str());
if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor,
req.max_acceleration_scaling_factor))
{
RCLCPP_WARN(LOGGER, "Time parametrization for the solution path failed.");
result = false;
moveit_code = moveit::core::MoveItErrorCode::FAILURE;
}
}

return result;
return moveit_code;
}

private:
Expand Down
16 changes: 9 additions & 7 deletions moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,21 +60,23 @@ class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRe
return "Add Ruckig trajectory smoothing.";
}

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
moveit::core::MoveItErrorCode 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
{
bool result = planner(planning_scene, req, res);
if (result && res.trajectory_)
moveit::core::MoveItErrorCode moveit_code = planner(planning_scene, req, res);
if (bool(moveit_code) && res.trajectory_)
{
if (!smoother_.applySmoothing(*res.trajectory_, req.max_velocity_scaling_factor,
req.max_acceleration_scaling_factor))
{
result = false;
moveit_code = moveit::core::MoveItErrorCode::FAILURE;
}
}

return result;
return moveit_code;
}

private:
Expand Down
Loading

0 comments on commit 66a64b4

Please sign in to comment.