Using ExecuteTaskSolutionCapability to execute a solution is overriding the joint state values

[JafarAbdi]

This is happening because in execute_task_solution_capability.cpp#L179 it modifies the state in the planning scene monitor which overrides the values from /joint_states, /tf, /tf_static topics

Possible fixes could be

1- Reset the robot_state's joint_state and multi_dof_joint_state for scene_diff before applying the planning scene

auto scene_diff = sub_traj.scene_diff; // Not ideal since we will copy the whole scene_diff before reseting joint_state & multi_dof_joint_state
scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();

2- In task.cpp#L268-L269 after we fill up the message we reset those variables

Let me know which solution you prefer so I open a PR with the patch

[JafarAbdi]

@v4hn @rhaschke do you any preferences?

[rhaschke]

I guess the differences between actual joint state values (after execution) and those in scene_diff are marginal, right? I would prefer the second option: the planning scene diff shouldn't comprise joint values in first place. Sorry for the delay in replying.

[sjahr]

I think this is addressed with #498. Please re-open if not.

[mechwiz]

While #498 addresses the issue in option 2, that is only true if the execute function is called within MTC. However, if a goal is sent to the execution_task_solution server directly (separate from within the execute function in MTC), then the joint values in that goal's trajectories will still override the joint state values. Hence I think option 1 is really the way to go to address this issue.

[rhaschke]

The problem with #498 is simply that the cleanup of the scene_diff's joint states is only performed in Task::execute, but not in the message generation. I filed PR #504, which should solve that issue.