Skip to content

Commit 32d747c

Browse files
sjahrsjahr
committed
Use MoveItErrorCode and revert some changes for discussion
1 parent e1b891b commit 32d747c

12 files changed

+170
-147
lines changed

‎core/include/moveit/task_constructor/solvers/cartesian_path.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -63,11 +63,11 @@ class CartesianPath : public PlannerInterface
6363

6464
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6565

66-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
66+
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
6767
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
6868
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6969

70-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
70+
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
7171
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
7272
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
7373
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

‎core/include/moveit/task_constructor/solvers/joint_interpolation.h

+9-7
Original file line numberDiff line numberDiff line change
@@ -58,14 +58,16 @@ class JointInterpolationPlanner : public PlannerInterface
5858

5959
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6060

61-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
62-
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
63-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
61+
MoveItErrorCode
62+
plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
63+
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
64+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6465

65-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
66-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
67-
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
66+
MoveItErrorCode
67+
plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
68+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
69+
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
70+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6971

7072
std::string getPlannerId() const override { return "JointInterpolationPlanner"; }
7173
};

‎core/include/moveit/task_constructor/solvers/multi_planner.h

+9-7
Original file line numberDiff line numberDiff line change
@@ -63,14 +63,16 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne
6363

6464
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6565

66-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
67-
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
66+
MoveItErrorCode
67+
plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
68+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
69+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6970

70-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
71-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
72-
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
73-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
71+
MoveItErrorCode
72+
plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
73+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
74+
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
75+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
7476
};
7577
} // namespace solvers
7678
} // namespace task_constructor

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h

+42-26
Original file line numberDiff line numberDiff line change
@@ -59,10 +59,10 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
5959
class PipelinePlanner : public PlannerInterface
6060
{
6161
public:
62-
/** Simple Constructor to use only a single pipeline
62+
/** \brief Simple Constructor to use only a single pipeline
6363
* \param [in] node ROS 2 node
6464
* \param [in] pipeline_name Name of the planning pipeline to be used.
65-
* This is also the assumed namespace where the parameters of this pipeline can be found
65+
* This is also the assumed namespace where the parameters of this pipeline can be found.
6666
* \param [in] planner_id Planner id to be used for planning. Empty string means default.
6767
*/
6868
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
@@ -71,13 +71,16 @@ class PipelinePlanner : public PlannerInterface
7171

7272
/** \brief Constructor
7373
* \param [in] node ROS 2 node
74-
* \param [in] pipeline_id_planner_id_map map containing pairs of pipeline and plugin names to be used for planning
75-
* \param [in] stopping_criterion_callback callback to decide when to stop parallel planning
76-
* \param [in] solution_selection_function callback to choose the best solution from multiple ran pipelines
74+
* \param [in] pipeline_id_planner_id_map Map containing pairs of pipeline and plugin names to be used for planning
75+
* \param [in] planning_pipelines Optional: A map with the names and initialized corresponding planning pipelines
76+
* \param [in] stopping_criterion_callback Optional: Callback to decide when to stop parallel planning
77+
* \param [in] solution_selection_function Optional: Callback to choose the best solution from multiple ran pipelines
7778
*/
7879
PipelinePlanner(
7980
const rclcpp::Node::SharedPtr& node,
8081
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
82+
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines =
83+
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>(),
8184
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
8285
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
8386
&moveit::planning_pipeline_interfaces::getShortestSolution);
@@ -101,7 +104,9 @@ class PipelinePlanner : public PlannerInterface
101104
void setSolutionSelectionFunction(
102105
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);
103106

104-
/** \brief If not yet done, initialize pipelines from pipeline_id_planner_id_map
107+
/** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are
108+
* configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are
109+
* initialized with the given robot model.
105110
* \param [in] robot_model robot model used to initialize the planning pipelines of this solver
106111
*/
107112
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
@@ -113,14 +118,15 @@ class PipelinePlanner : public PlannerInterface
113118
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
114119
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
115120
* \param [in] path_constraints Path contraints for the planning problem
116-
* \return true If the solver found a successful solution for the given planning problem
121+
* \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise,
122+
* return an appropriate error type and message explaining why it failed.
117123
*/
118-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
119-
const core::JointModelGroup* joint_model_group, double timeout,
120-
robot_trajectory::RobotTrajectoryPtr& result,
121-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
124+
MoveItErrorCode
125+
plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
126+
const core::JointModelGroup* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
127+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
122128

123-
/** \brief Plan a trajectory from a planning scene 'from' to a Cartesian target pose with an offset
129+
/** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset
124130
* \param [in] from Start planning scene
125131
* \param [in] link Link for which a target pose is given
126132
* \param [in] offset Offset to be applied to a given target pose
@@ -129,18 +135,27 @@ class PipelinePlanner : public PlannerInterface
129135
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
130136
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
131137
* \param [in] path_constraints Path contraints for the planning problem
132-
* \return true If the solver found a successful solution for the given planning problem
138+
* \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise,
139+
* return an appropriate error type and message explaining why it failed.
140+
*/
141+
MoveItErrorCode
142+
plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
143+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
144+
const moveit::core::JointModelGroup* joint_model_group, double timeout,
145+
robot_trajectory::RobotTrajectoryPtr& result,
146+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
147+
148+
/**
149+
* \brief Get planner name
150+
* \return Name of the last successful planner
133151
*/
134-
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
135-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
136-
const moveit::core::JointModelGroup* joint_model_group, double timeout,
137-
robot_trajectory::RobotTrajectoryPtr& result,
138-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
139-
140152
std::string getPlannerId() const override { return last_successful_planner_; }
141153

142154
protected:
143-
/** \brief Actual plan() implementation, targeting the given goal_constraints.
155+
/** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by
156+
* the public plan function after the goal constraints are generated. This function uses a predefined number of
157+
* planning pipelines in parallel to solve the planning problem and choose the best (user-defined) solution.
158+
* \param [in] planning_scene Scene for which the planning should be solved
144159
* \param [in] planning_scene Scene for which the planning should be solved
145160
* \param [in] joint_model_group Group of joints for which this trajectory is created
146161
* \param [in] goal_constraints Set of constraints that need to be satisfied by the solution
@@ -149,17 +164,18 @@ class PipelinePlanner : public PlannerInterface
149164
* \param [in] path_constraints Path contraints for the planning problem
150165
* \return true if the solver found a successful solution for the given planning problem
151166
*/
152-
Result plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
153-
const moveit::core::JointModelGroup* joint_model_group,
154-
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
155-
robot_trajectory::RobotTrajectoryPtr& result,
156-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
167+
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
168+
const moveit::core::JointModelGroup* joint_model_group,
169+
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
170+
robot_trajectory::RobotTrajectoryPtr& result,
171+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
157172

158173
rclcpp::Node::SharedPtr node_;
159174

160175
std::string last_successful_planner_;
161176

162-
/** \brief Map of instantiated (and named) planning pipelines. */
177+
/** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion
178+
* planning pipeline when during plan(..) */
163179
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
164180

165181
moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_;

‎core/include/moveit/task_constructor/solvers/planner_interface.h

+13-17
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
#include <moveit/macros/class_forward.h>
4242
#include <moveit_msgs/msg/constraints.hpp>
4343
#include <moveit/task_constructor/properties.h>
44+
#include <moveit/utils/moveit_error_code.h>
4445
#include <Eigen/Geometry>
4546

4647
namespace planning_scene {
@@ -64,21 +65,16 @@ namespace moveit {
6465
namespace task_constructor {
6566
namespace solvers {
6667

68+
using MoveItErrorCode = moveit::core::MoveItErrorCode;
69+
using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes;
70+
6771
MOVEIT_CLASS_FORWARD(PlannerInterface);
6872
class PlannerInterface
6973
{
7074
// these properties take precedence over stage properties
7175
PropertyMap properties_;
7276

7377
public:
74-
struct Result
75-
{
76-
bool success;
77-
std::string message;
78-
79-
operator bool() const { return success; }
80-
};
81-
8278
PlannerInterface();
8379
virtual ~PlannerInterface() {}
8480

@@ -96,17 +92,17 @@ class PlannerInterface
9692
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
9793

9894
/// plan trajectory between to robot states
99-
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from,
100-
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
101-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
102-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
95+
virtual MoveItErrorCode
96+
plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
97+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
98+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
10399

104100
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
105-
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
106-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
107-
const moveit::core::JointModelGroup* jmg, double timeout,
108-
robot_trajectory::RobotTrajectoryPtr& result,
109-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
101+
virtual MoveItErrorCode
102+
plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
103+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
104+
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
105+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
110106

111107
// get name of the planner
112108
virtual std::string getPlannerId() const = 0;

‎core/src/solvers/cartesian_path.cpp

+13-12
Original file line numberDiff line numberDiff line change
@@ -63,11 +63,11 @@ CartesianPath::CartesianPath() {
6363

6464
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
6565

66-
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
67-
const planning_scene::PlanningSceneConstPtr& to,
68-
const moveit::core::JointModelGroup* jmg, double timeout,
69-
robot_trajectory::RobotTrajectoryPtr& result,
70-
const moveit_msgs::msg::Constraints& path_constraints) {
66+
MoveItErrorCode CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
67+
const planning_scene::PlanningSceneConstPtr& to,
68+
const moveit::core::JointModelGroup* jmg, double timeout,
69+
robot_trajectory::RobotTrajectoryPtr& result,
70+
const moveit_msgs::msg::Constraints& path_constraints) {
7171
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
7272
if (!link)
7373
return { false, "no unique tip for joint model group: " + jmg->getName() };
@@ -77,11 +77,11 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
7777
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
7878
}
7979

80-
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
81-
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
82-
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
83-
double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
84-
const moveit_msgs::msg::Constraints& path_constraints) {
80+
MoveItErrorCode CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
81+
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
82+
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
83+
double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
84+
const moveit_msgs::msg::Constraints& path_constraints) {
8585
const auto& props = properties();
8686
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
8787

@@ -114,9 +114,10 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
114114
props.get<double>("max_acceleration_scaling_factor"));
115115

116116
if (achieved_fraction < props.get<double>("min_fraction")) {
117-
return { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
117+
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
118+
"Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
118119
}
119-
return { true, "achieved fraction: " + std::to_string(achieved_fraction) };
120+
return MoveItErrorCode(MoveItErrorCodes::SUCCESS, "achieved fraction: " + std::to_string(achieved_fraction));
120121
}
121122
} // namespace solvers
122123
} // namespace task_constructor

0 commit comments

Comments
 (0)