Use tl expected

moveit · Jan 18, 2024 · 2ffacf8 · 2ffacf8
1 parent 57e8490
commit 2ffacf8
Show file tree

Hide file tree

Showing 14 changed files with 108 additions and 76 deletions.
diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(moveit_task_constructor_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rviz_marker_tools REQUIRED)
 find_package(tf2_eigen REQUIRED)
+find_package(tl_expected REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
 set(CMAKE_CXX_STANDARD 17)
@@ -44,5 +45,6 @@ ament_export_dependencies(moveit_task_constructor_msgs)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rviz_marker_tools)
 ament_export_dependencies(tf2_eigen)
+ament_export_dependencies(tl_expected)
 ament_export_dependencies(visualization_msgs)
 ament_package()
diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h
@@ -63,11 +63,11 @@ class CartesianPath : public PlannerInterface
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	tl::expected<bool, std::string> plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
 	          const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+	tl::expected<bool, std::string> plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
 	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
 	          double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h
@@ -58,14 +58,16 @@ class JointInterpolationPlanner : public PlannerInterface
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
-	          const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	tl::expected<bool, std::string>
+	plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	     const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
-	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
-	          double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	tl::expected<bool, std::string>
+	plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+	     const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
+	     double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
 	std::string getPlannerId() const override { return std::string("JointInterpolationPlanner"); }
 };

diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h
@@ -63,14 +63,16 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
-	          const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	tl::expected<bool, std::string>
+	plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	     const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
-	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
-	          double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	tl::expected<bool, std::string>
+	plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+	     const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
+	     double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 };
 }  // namespace solvers
 }  // namespace task_constructor

diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h
@@ -126,7 +126,7 @@ class PipelinePlanner : public PlannerInterface
 	 * \param [in] path_constraints Path contraints for the planning problem
 	 * \return true If the solver found a successful solution for the given planning problem
 	 */
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	tl::expected<bool, std::string> plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
 	          const core::JointModelGroup* joint_model_group, double timeout,
 	          robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
@@ -142,7 +142,7 @@ class PipelinePlanner : public PlannerInterface
 	 * \param [in] path_constraints Path contraints for the planning problem
 	 * \return true If the solver found a successful solution for the given planning problem
 	 */
-	bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+	tl::expected<bool, std::string> plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
 	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
 	          const moveit::core::JointModelGroup* joint_model_group, double timeout,
 	          robot_trajectory::RobotTrajectoryPtr& result,
@@ -170,7 +170,7 @@ class PipelinePlanner : public PlannerInterface
 	 * problem
 	 * \return true If the solver found a successful solution for the given planning problem
 	 */
-	bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
+	tl::expected<bool, std::string> plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
 	          const moveit::core::JointModelGroup* joint_model_group,
 	          const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
 	          robot_trajectory::RobotTrajectoryPtr& result,

diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h
@@ -42,6 +42,7 @@
 #include <moveit_msgs/msg/constraints.hpp>
 #include <moveit/task_constructor/properties.h>
 #include <Eigen/Geometry>
+#include <tl_expected/expected.hpp>
 
 namespace planning_scene {
 MOVEIT_CLASS_FORWARD(PlanningScene);
@@ -88,17 +89,17 @@ class PlannerInterface
 	virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
 
 	/// plan trajectory between to robot states
-	virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
-	                  const moveit::core::JointModelGroup* jmg, double timeout,
-	                  robot_trajectory::RobotTrajectoryPtr& result,
-	                  const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
+	virtual tl::expected<bool, std::string>
+	plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	     const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
 
 	/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
-	virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
-	                  const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
-	                  const moveit::core::JointModelGroup* jmg, double timeout,
-	                  robot_trajectory::RobotTrajectoryPtr& result,
-	                  const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
+	virtual tl::expected<bool, std::string>
+	plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+	     const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
+	     double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
 
 	// get name of the planner
 	virtual std::string getPlannerId() const = 0;

diff --git a/core/package.xml b/core/package.xml
@@ -22,6 +22,7 @@
 	<depend>rclcpp</depend>
 	<depend>rviz_marker_tools</depend>
 	<depend>tf2_eigen</depend>
+	<depend>tl_expected</depend>
 	<depend>visualization_msgs</depend>
 
 	<test_depend>ament_cmake_gmock</test_depend>

diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp
@@ -63,10 +63,11 @@ CartesianPath::CartesianPath() {
 
 void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
 
-bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
-                         const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
-                         double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-                         const moveit_msgs::msg::Constraints& path_constraints) {
+tl::expected<bool, std::string> CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
+                                                    const planning_scene::PlanningSceneConstPtr& to,
+                                                    const moveit::core::JointModelGroup* jmg, double timeout,
+                                                    robot_trajectory::RobotTrajectoryPtr& result,
+                                                    const moveit_msgs::msg::Constraints& path_constraints) {
 	const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
 	if (!link) {
 		RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName());
@@ -78,11 +79,12 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
 	            std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
 }
 
-bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
-                         const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
-                         const moveit::core::JointModelGroup* jmg, double /*timeout*/,
-                         robot_trajectory::RobotTrajectoryPtr& result,
-                         const moveit_msgs::msg::Constraints& path_constraints) {
+tl::expected<bool, std::string> CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
+                                                    const moveit::core::LinkModel& link,
+                                                    const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
+                                                    const moveit::core::JointModelGroup* jmg, double /*timeout*/,
+                                                    robot_trajectory::RobotTrajectoryPtr& result,
+                                                    const moveit_msgs::msg::Constraints& path_constraints) {
 	const auto& props = properties();
 	planning_scene::PlanningScenePtr sandbox_scene = from->diff();
 

diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp
@@ -59,11 +59,10 @@ JointInterpolationPlanner::JointInterpolationPlanner() {
 
 void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
 
-bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
-                                     const planning_scene::PlanningSceneConstPtr& to,
-                                     const moveit::core::JointModelGroup* jmg, double /*timeout*/,
-                                     robot_trajectory::RobotTrajectoryPtr& result,
-                                     const moveit_msgs::msg::Constraints& /*path_constraints*/) {
+tl::expected<bool, std::string> JointInterpolationPlanner::plan(
+    const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+    const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
+    const moveit_msgs::msg::Constraints& /*path_constraints*/) {
 	const auto& props = properties();
 
 	// Get maximum joint distance
@@ -116,11 +115,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
 	return true;
 }
 
-bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
-                                     const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
-                                     const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
-                                     double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-                                     const moveit_msgs::msg::Constraints& path_constraints) {
+tl::expected<bool, std::string>
+JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+                                const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
+                                const moveit::core::JointModelGroup* jmg, double timeout,
+                                robot_trajectory::RobotTrajectoryPtr& result,
+                                const moveit_msgs::msg::Constraints& path_constraints) {
 	timeout = std::min(timeout, properties().get<double>("timeout"));
 	const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
 

diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp
@@ -49,10 +49,11 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) {
 		p->init(robot_model);
 }
 
-bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
-                        const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
-                        double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-                        const moveit_msgs::msg::Constraints& path_constraints) {
+tl::expected<bool, std::string> MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
+                                                   const planning_scene::PlanningSceneConstPtr& to,
+                                                   const moveit::core::JointModelGroup* jmg, double timeout,
+                                                   robot_trajectory::RobotTrajectoryPtr& result,
+                                                   const moveit_msgs::msg::Constraints& path_constraints) {
 	double remaining_time = std::min(timeout, properties().get<double>("timeout"));
 	auto start_time = std::chrono::steady_clock::now();
 
@@ -71,11 +72,12 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
 	return false;
 }
 
-bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
-                        const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
-                        const moveit::core::JointModelGroup* jmg, double timeout,
-                        robot_trajectory::RobotTrajectoryPtr& result,
-                        const moveit_msgs::msg::Constraints& path_constraints) {
+tl::expected<bool, std::string> MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
+                                                   const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
+                                                   const Eigen::Isometry3d& target,
+                                                   const moveit::core::JointModelGroup* jmg, double timeout,
+                                                   robot_trajectory::RobotTrajectoryPtr& result,
+                                                   const moveit_msgs::msg::Constraints& path_constraints) {
 	double remaining_time = std::min(timeout, properties().get<double>("timeout"));
 	auto start_time = std::chrono::steady_clock::now();