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

* Reset scene diff in task

* Add explanation for scene reset
This commit is contained in:
Sebastian Jahr 2023-10-24 10:39:13 +02:00 committed by GitHub
parent b25d2ba318
commit fb02c7f202
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -281,6 +281,13 @@ 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);