Clear JointStates in scene diff (moveit#504)

rhaschke · rhaschke · commit a75d50e0ede3 · 2024-03-07T12:23:07.000+01:00
Joints are handled in trajectories. Scene diffs should not modify joints during execution. Fixes moveit#353.
diff --git a/core/src/storage.cpp b/core/src/storage.cpp
@@ -230,6 +230,9 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros
 		trajectory()->getRobotTrajectoryMsg(t.trajectory);
 
 	this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
+	// reset JointStates (joints are already handled in trajectories)
+	t.scene_diff.robot_state.joint_state = sensor_msgs::JointState();
+	t.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();
 }
 
 double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {

Original file line number	Diff line number	Diff line change
`@@ -230,6 +230,9 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros`
`230`	`230`	`trajectory()->getRobotTrajectoryMsg(t.trajectory);`
`231`	`231`
`232`	`232`	`this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);`
	`233`	`+ // reset JointStates (joints are already handled in trajectories)`
	`234`	`+ t.scene_diff.robot_state.joint_state = sensor_msgs::JointState();`
	`235`	`+ t.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();`
`233`	`236`	`}`
`234`	`237`
`235`	`238`	`double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {`