Silent error "Found empty JointState message"

This commit is contained in:
Robert Haschke 2024-07-18 13:45:15 +02:00
parent 7631486648
commit 4f69a22ddb

View File

@ -170,8 +170,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
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);