From af9c82a20ffef0ae85d62d519833dd209a91e0e3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 9 Feb 2025 12:23:17 +0100 Subject: [PATCH] Provide action feedback during task execution --- .../src/execute_task_solution_capability.cpp | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 17facf74c..01a5ab5d2 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -166,21 +166,26 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory); exec_traj.controller_names_ = sub_traj.execution_info.controller_names; - /* TODO add action feedback and markers */ - exec_traj.effect_on_success_ = [this, - &scene_diff = const_cast<::moveit_msgs::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::JointState(); - scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState(); - scene_diff.robot_state.is_diff = true; // silent empty JointState msg error - - if (!moveit::core::isEmpty(scene_diff)) { - ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "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::PlanningScene&>(sub_traj.scene_diff), description, i, + no = solution.sub_trajectory.size()](const plan_execution::ExecutableMotionPlan* /*plan*/) { + // publish feedback + moveit_task_constructor_msgs::ExecuteTaskSolutionFeedback feedback; + feedback.sub_id = i; + feedback.sub_no = no; + as_->publishFeedback(feedback); + + // Never modify joint state directly (only via robot trajectories) + scene_diff.robot_state.joint_state = sensor_msgs::JointState(); + scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState(); + scene_diff.robot_state.is_diff = true; // silent empty JointState msg error + + if (!moveit::core::isEmpty(scene_diff)) { + ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "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)) {