Merge branch 'master' into ros2

rhaschke · rhaschke · commit dd74b59061b6 · 2025-02-11T08:59:27.000+01:00
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -111,7 +111,7 @@ void ExecuteTaskSolutionCapability::execCallback(
 	}
 
 	plan_execution::ExecutableMotionPlan plan;
-	if (!constructMotionPlan(goal->solution, plan))
+	if (!constructMotionPlan(goal->solution, plan, goal_handle))
 		result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
 	else {
 		RCLCPP_INFO(LOGGER, "Executing TaskSolution");
@@ -133,8 +133,9 @@ rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback(
 	return rclcpp_action::CancelResponse::ACCEPT;
 }
 
-bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
-                                                        plan_execution::ExecutableMotionPlan& plan) {
+bool ExecuteTaskSolutionCapability::constructMotionPlan(
+    const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
 	moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
 
 	moveit::core::RobotState state(model);
@@ -182,20 +183,26 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		exec_traj.controller_name = sub_traj.execution_info.controller_names;
 
 		/* TODO add action feedback and markers */
-		exec_traj.effect_on_success = [this,
-		                               &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
-		                               description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
-			// Never modify joint state directly (only via robot trajectories)
-			scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
-			scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
-			scene_diff.robot_state.is_diff = true;  // silent empty JointState msg error
-
-			if (!moveit::core::isEmpty(scene_diff)) {
-				RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
-				return context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff);
-			}
-			return true;
-		};
+		exec_traj.effect_on_success =
+		    [this, &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff), description,
+		     goal_handle, i, no = solution.sub_trajectory.size()](const plan_execution::ExecutableMotionPlan* /*plan*/) {
+			    // publish feedback
+			    auto feedback = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Feedback>();
+			    feedback->sub_id = i;
+			    feedback->sub_no = no;
+			    goal_handle->publish_feedback(feedback);
+
+			    // Never modify joint state directly (only via robot trajectories)
+			    scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
+			    scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
+			    scene_diff.robot_state.is_diff = true;  // silent empty JointState msg error
+
+			    if (!moveit::core::isEmpty(scene_diff)) {
+				    RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
+				    return context_->planning_scene_monitor_->newPlanningSceneMessage(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)) {
diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h
@@ -60,8 +60,10 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability
 private:
 	using ExecuteTaskSolutionAction = moveit_task_constructor_msgs::action::ExecuteTaskSolution;
 	using ActionServerType = rclcpp_action::Server<ExecuteTaskSolutionAction>;
-	bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
-	                         plan_execution::ExecutableMotionPlan& plan);
+	bool
+	constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
+	                    plan_execution::ExecutableMotionPlan& plan,
+	                    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
 
 	void execCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
 
diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h
@@ -80,7 +80,7 @@ class ContainerBase : public Stage
 
 	virtual bool canCompute() const = 0;
 	virtual void compute() = 0;
-	void explainFailure(std::ostream& os) const override;
+	bool explainFailure(std::ostream& os) const override;
 
 	/// called by a (direct) child when a new solution becomes available
 	virtual void onNewSolution(const SolutionBase& s) = 0;
diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h
@@ -242,7 +242,7 @@ class Stage
 	/// Should we generate failure solutions? Note: Always report a failure!
 	bool storeFailures() const;
 
-	virtual void explainFailure(std::ostream& os) const;
+	virtual bool explainFailure(std::ostream& /*os*/) const { return false; };
 
 	/// Get the stage's property map
 	PropertyMap& properties();
diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h
@@ -139,7 +139,7 @@ class Task : protected WrapperBase
 	void printState(std::ostream& os = std::cout) const;
 
 	/// print an explanation for a planning failure to os
-	void explainFailure(std::ostream& os = std::cout) const override;
+	bool explainFailure(std::ostream& os = std::cout) const override;
 
 	size_t numSolutions() const { return solutions().size(); }
 	const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
diff --git a/core/src/container.cpp b/core/src/container.cpp
@@ -453,18 +453,21 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
 		throw errors;
 }
 
-void ContainerBase::explainFailure(std::ostream& os) const {
+bool ContainerBase::explainFailure(std::ostream& os) const {
 	for (const auto& stage : pimpl()->children()) {
 		if (!stage->solutions().empty())
 			continue;  // skip deeper traversal, this stage produced solutions
 		if (stage->numFailures()) {
 			os << stage->name() << " (0/" << stage->numFailures() << ")";
-			stage->explainFailure(os);
+			if (!stage->failures().empty())
+				os << ": " << stage->failures().front()->comment();
 			os << '\n';
-			break;
+			return true;
 		}
-		stage->explainFailure(os);  // recursively process children
+		if (stage->explainFailure(os))  // recursively process children
+			return true;
 	}
+	return false;
 }
 
 std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
diff --git a/core/src/stage.cpp b/core/src/stage.cpp
@@ -437,11 +437,6 @@ bool Stage::storeFailures() const {
 	return pimpl()->storeFailures();
 }
 
-void Stage::explainFailure(std::ostream& os) const {
-	if (!failures().empty())
-		os << ": " << failures().front()->comment();
-}
-
 PropertyMap& Stage::properties() {
 	return pimpl()->properties_;
 }
diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
@@ -258,16 +258,23 @@ void ComputeIK::compute() {
 	const moveit::core::JointModelGroup* jmg = nullptr;
 	std::string msg;
 
+	auto report_failure = [&s, this](const std::string& msg) {
+		planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
+		SubTrajectory solution;
+		solution.markAsFailure(msg);
+		spawn(InterfaceState(scene), std::move(solution));
+	};
+
 	if (!validateEEF(props, robot_model, eef_jmg, &msg)) {
-		RCLCPP_WARN_STREAM(LOGGER, msg);
+		report_failure(msg);
 		return;
 	}
 	if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) {
-		RCLCPP_WARN_STREAM(LOGGER, msg);
+		report_failure(msg);
 		return;
 	}
 	if (!eef_jmg && !jmg) {
-		RCLCPP_WARN_STREAM(LOGGER, "Neither eef nor group are well defined");
+		report_failure("Neither eef nor group are well defined");
 		return;
 	}
 	properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
@@ -281,7 +288,7 @@ void ComputeIK::compute() {
 	tf2::fromMsg(target_pose_msg.pose, target_pose);
 	if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
 		if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
-			RCLCPP_WARN_STREAM(LOGGER, "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
+			report_failure(fmt::format("Unknown reference frame for target pose: '{}'", target_pose_msg.header.frame_id));
 			return;
 		}
 		// transform target_pose w.r.t. planning frame
@@ -296,7 +303,7 @@ void ComputeIK::compute() {
 		//  determine IK link from eef/group
 		if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
 		                       jmg->getOnlyOneEndEffectorTip())) {
-			RCLCPP_WARN_STREAM(LOGGER, "Failed to derive IK target link");
+			report_failure("Failed to derive IK target link");
 			return;
 		}
 		ik_pose_msg.header.frame_id = link->getName();
@@ -307,7 +314,7 @@ void ComputeIK::compute() {
 		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
 
 		if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
-			RCLCPP_WARN_STREAM(LOGGER, fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id));
+			report_failure(fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id));
 			return;
 		}
 		ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp
@@ -36,6 +36,7 @@
 /* Authors: Michael Goerner, Luca Lach, Robert Haschke */
 
 #include <chrono>
+#include <fmt/format.h>
 
 #include <moveit/task_constructor/stages/current_state.h>
 #include <moveit/task_constructor/storage.h>
@@ -101,7 +102,13 @@ void CurrentState::compute() {
 			return;
 		}
 	}
-	RCLCPP_WARN(LOGGER, "failed to acquire current PlanningScene");
+	if (storeFailures()) {
+		SubTrajectory solution;
+		solution.markAsFailure(
+		    fmt::format("Failed to acquire current PlanningScene within timeout of {}s", timeout.count()));
+		spawn(InterfaceState(scene_), std::move(solution));
+	} else
+		RCLCPP_WARN(LOGGER, "failed to acquire current PlanningScene");
 }
 }  // namespace stages
 }  // namespace task_constructor
diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp
@@ -34,6 +34,8 @@
 
 /* Authors: Robert Haschke */
 
+#include <fmt/format.h>
+
 #include <moveit/task_constructor/stages/fixed_cartesian_poses.h>
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/cost_terms.h>
@@ -88,7 +90,12 @@ void FixedCartesianPoses::compute() {
 		if (pose.header.frame_id.empty())
 			pose.header.frame_id = scene->getPlanningFrame();
 		else if (!scene->knowsFrameTransform(pose.header.frame_id)) {
-			RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", pose.header.frame_id.c_str());
+			if (storeFailures()) {
+				SubTrajectory trajectory;
+				trajectory.markAsFailure(fmt::format("Unknown frame: '{}'", pose.header.frame_id));
+				spawn(InterfaceState(scene), std::move(trajectory));
+			} else
+				RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", pose.header.frame_id.c_str());
 			continue;
 		}
 
diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp
@@ -40,6 +40,7 @@
 #include <moveit/task_constructor/marker_tools.h>
 #include <moveit/planning_scene/planning_scene.hpp>
 #include <rviz_marker_tools/marker_creation.h>
+#include <fmt/format.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -79,7 +80,12 @@ void GeneratePose::compute() {
 	if (target_pose.header.frame_id.empty())
 		target_pose.header.frame_id = scene->getPlanningFrame();
 	else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
-		RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
+		if (storeFailures()) {
+			SubTrajectory trajectory;
+			trajectory.markAsFailure(fmt::format("Unknown frame: '{}'", target_pose.header.frame_id));
+			spawn(InterfaceState(scene), std::move(trajectory));
+		} else
+			RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
 		return;
 	}
 
diff --git a/core/src/stages/generate_random_pose.cpp b/core/src/stages/generate_random_pose.cpp
@@ -47,6 +47,7 @@
 #include <tf2_eigen/tf2_eigen.h>
 #endif
 
+#include <fmt/format.h>
 #include <chrono>
 
 static auto LOGGER = rclcpp::get_logger("GenerateRandomPose");
@@ -100,7 +101,12 @@ void GenerateRandomPose::compute() {
 	if (seed_pose.header.frame_id.empty())
 		seed_pose.header.frame_id = scene->getPlanningFrame();
 	else if (!scene->knowsFrameTransform(seed_pose.header.frame_id)) {
-		RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", seed_pose.header.frame_id.c_str());
+		if (storeFailures()) {
+			SubTrajectory trajectory;
+			trajectory.markAsFailure(fmt::format("Unknown frame: '{}'", seed_pose.header.frame_id));
+			spawn(InterfaceState(scene), std::move(trajectory));
+		} else
+			RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", seed_pose.header.frame_id.c_str());
 		return;
 	}
 
diff --git a/core/src/task.cpp b/core/src/task.cpp
@@ -364,9 +364,9 @@ void Task::printState(std::ostream& os) const {
 	os << *stages();
 }
 
-void Task::explainFailure(std::ostream& os) const {
+bool Task::explainFailure(std::ostream& os) const {
 	os << "Failing stage(s):\n";
-	stages()->explainFailure(os);
+	return stages()->explainFailure(os);
 }
 }  // namespace task_constructor
 }  // namespace moveit

Original file line number	Diff line number	Diff line change
`@@ -453,18 +453,21 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {`
`453`	`453`	`throw errors;`
`454`	`454`	`}`
`455`	`455`
`456`		`-void ContainerBase::explainFailure(std::ostream& os) const {`
	`456`	`+bool ContainerBase::explainFailure(std::ostream& os) const {`
`457`	`457`	`for (const auto& stage : pimpl()->children()) {`
`458`	`458`	`if (!stage->solutions().empty())`
`459`	`459`	`continue; // skip deeper traversal, this stage produced solutions`
`460`	`460`	`if (stage->numFailures()) {`
`461`	`461`	`os << stage->name() << " (0/" << stage->numFailures() << ")";`
`462`		`- stage->explainFailure(os);`
	`462`	`+ if (!stage->failures().empty())`
	`463`	`+ os << ": " << stage->failures().front()->comment();`
`463`	`464`	`os << '\n';`
`464`		`- break;`
	`465`	`+ return true;`
`465`	`466`	`}`
`466`		`- stage->explainFailure(os); // recursively process children`
	`467`	`+ if (stage->explainFailure(os)) // recursively process children`
	`468`	`+ return true;`
`467`	`469`	`}`
	`470`	`+ return false;`
`468`	`471`	`}`
`469`	`472`
`470`	`473`	`std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {`
Original file line number	Diff line number	Diff line change
`@@ -437,11 +437,6 @@ bool Stage::storeFailures() const {`
`437`	`437`	`return pimpl()->storeFailures();`
`438`	`438`	`}`
`439`	`439`
`440`		`-void Stage::explainFailure(std::ostream& os) const {`
`441`		`- if (!failures().empty())`
`442`		`- os << ": " << failures().front()->comment();`
`443`		`-}`
`444`		`-`
`445`	`440`	`PropertyMap& Stage::properties() {`
`446`	`441`	`return pimpl()->properties_;`
`447`	`442`	`}`