fixup! Reset joint values in scene diff for execution (#498)

moveit · Nov 8, 2023 · 846e8f7 · 846e8f7
1 parent 81a5a6b
commit 846e8f7
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 7 deletions.
diff --git a/core/src/storage.cpp b/core/src/storage.cpp
@@ -209,6 +209,12 @@ void SolutionBase::markAsFailure(const std::string& msg) {
 void SolutionBase::toMsg(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const {
 	appendTo(msg, introspection);
 	start()->scene()->getPlanningSceneMsg(msg.start_scene);
+
+	// Reset JointStates in scene_diff (joints are explicitly handled in trajectories)
+	for (auto& sub_trajectory : goal.solution.sub_trajectory) {
+		sub_trajectory.scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
+		sub_trajectory.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
+	}
 }
 
 void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection) const {

diff --git a/core/src/task.cpp b/core/src/task.cpp
@@ -281,13 +281,6 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
 	moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal;
 	s.toMsg(goal.solution, pimpl()->introspection_.get());
 
-	// Reset joint values in scene diff to prevent any execution capability from
-	// modifying the planning scene joint values
-	for (auto& sub_trajectory : goal.solution.sub_trajectory) {
-		sub_trajectory.scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
-		sub_trajectory.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
-	}
-
 	moveit_msgs::msg::MoveItErrorCodes error_code;
 	error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
 	auto goal_handle_future = ac->async_send_goal(goal);