Remove rviz2 dependency from rviz_marker_tools

.github/workflows/ci.yaml

@@ -19,7 +19,6 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - IMAGE: humble-source
           - IMAGE: rolling-source
             NAME: ccov
             TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"

capabilities/src/execute_task_solution_capability.cpp

@@ -90,12 +90,22 @@ void ExecuteTaskSolutionCapability::initialize() {
 	// configure the action server
 	as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
 	    context_->moveit_cpp_->getNode(), "execute_task_solution",
-	    ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
-	                                             std::placeholders::_1, std::placeholders::_2)),
-	    ActionServerType::CancelCallback(
-	        std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
-	    ActionServerType::AcceptedCallback(
-	        std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
+	    [this](const rclcpp_action::GoalUUID& /*uuid*/,
+	           const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) {
+		    // Reject new goal if another goal is currently processed
+		    if (last_goal_future_.valid() &&
+		        last_goal_future_.wait_for(std::chrono::seconds::zero()) != std::future_status::ready) {
+			    return rclcpp_action::GoalResponse::REJECT;
+		    }
+		    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+	    },
+	    [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
+		    return preemptCallback(goal_handle);
+	    },
+	    [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
+		    last_goal_future_ =
+		        std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle);
+	    });
 }
 
 void ExecuteTaskSolutionCapability::goalCallback(
@@ -145,13 +155,11 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 	plan.plan_components.reserve(solution.sub_trajectory.size());
 	for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) {
 		const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i];
-
 		plan.plan_components.emplace_back();
 		plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components.back();
 
 		// define individual variable for use in closure below
-		const std::string description = std::to_string(i + 1) + "/" + std::to_string(solution.sub_trajectory.size());
-		exec_traj.description = description;
+		exec_traj.description = std::to_string(i + 1) + "/" + std::to_string(solution.sub_trajectory.size());
 
 		const moveit::core::JointModelGroup* group = nullptr;
 		{
@@ -170,20 +178,29 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		}
 		exec_traj.trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
 		exec_traj.trajectory->setRobotTrajectoryMsg(state, sub_traj.trajectory);
+		// Check that sub trajectories that contain a valid trajectory have controllers configured.
+		if (!sub_traj.trajectory.joint_trajectory.points.empty() && sub_traj.execution_info.controller_names.empty()) {
+			RCLCPP_WARN(LOGGER,
+			            "The trajectory of stage '%i' from task '%s' does not have any controllers specified for "
+			            "trajectory execution. This might lead to unexpected controller selection.",
+			            sub_traj.info.stage_id, solution.task_id.c_str());
+		}
+		exec_traj.controller_name = sub_traj.execution_info.controller_names;
 
 		/* TODO add action feedback and markers */
-		exec_traj.effect_on_success = [this, sub_traj,
-		                               description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
-			if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
-				RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
-				return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
-			}
-			return true;
-		};
+		exec_traj.effect_on_success =
+		    [this, sub_traj, description = exec_traj.description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
+			    if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
+				    RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
+				    return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
+			    }
+			    return true;
+		    };
 
 		if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
 		    !moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
-			RCLCPP_ERROR_STREAM(LOGGER, "invalid intermediate robot state in scene diff of SubTrajectory " << description);
+			RCLCPP_ERROR_STREAM(LOGGER, "invalid intermediate robot state in scene diff of SubTrajectory "
+			                                << exec_traj.description);
 			return false;
 		}
 	}

capabilities/src/execute_task_solution_capability.h

@@ -68,13 +68,8 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability
 	rclcpp_action::CancelResponse
 	preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
 
-	/** Always accept the goal */
-	rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& /*uuid*/,
-	                                          const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) const {
-		return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-	}
-
 	ActionServerType::SharedPtr as_;
+	std::future<void> last_goal_future_;
 };
 
 }  // namespace move_group

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

@@ -63,14 +63,16 @@ 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,
+	MoveItErrorCode 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,
+	MoveItErrorCode 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("CartesianPath"); }
 };
 }  // namespace solvers
 }  // namespace task_constructor

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

@@ -58,14 +58,18 @@ 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;
+	MoveItErrorCode
+	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;
+	MoveItErrorCode
+	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"); }
 };
 }  // namespace solvers
 }  // namespace task_constructor

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;
+	MoveItErrorCode
+	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;
+	MoveItErrorCode
+	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