Skip to content

Commit 4f69a22

Browse files
committed
Silent error "Found empty JointState message"
1 parent 7631486 commit 4f69a22

File tree

1 file changed

+2
-0
lines changed

1 file changed

+2
-0
lines changed

capabilities/src/execute_task_solution_capability.cpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -170,8 +170,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
170170
exec_traj.effect_on_success_ = [this,
171171
&scene_diff = const_cast<::moveit_msgs::PlanningScene&>(sub_traj.scene_diff),
172172
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
173+
// Never modify joint state directly (only via robot trajectories)
173174
scene_diff.robot_state.joint_state = sensor_msgs::JointState();
174175
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();
176+
scene_diff.robot_state.is_diff = true; // silent empty JointState msg error
175177

176178
if (!moveit::core::isEmpty(scene_diff)) {
177179
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);

0 commit comments

Comments
 (0)