Fix Solution::fillMessage() (#432)

The scene_diff field usually describes PlanningScene changes of the end scene relative to the start scene.
For backwards planning, this direction is reversed: the start scene is derived from the end scene.
Thus, we need to generate a full planning scene message for the end scene if planning progressed backwards.

Fixes #405.
This commit is contained in:
Robert Haschke 2024-03-08 17:22:41 +01:00
commit 795bde7b62
3 changed files with 13 additions and 9 deletions

View File

@ -170,11 +170,15 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
exec_traj.controller_names_ = sub_traj.execution_info.controller_names;
/* TODO add action feedback and markers */
exec_traj.effect_on_success_ = [this, sub_traj,
exec_traj.effect_on_success_ = [this,
&scene_diff = const_cast<::moveit_msgs::PlanningScene&>(sub_traj.scene_diff),
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
scene_diff.robot_state.joint_state = sensor_msgs::JointState();
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();
if (!moveit::core::isEmpty(scene_diff)) {
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
return context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff);
}
return true;
};

View File

@ -81,7 +81,7 @@ class TestModifyPlanningScene(unittest.TestCase):
s = self.task.solutions[0].toMsg()
self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box")
def DISABLED_test_bw_add_object(self):
def test_bw_add_object(self):
# add object to move_group's planning scene
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
@ -104,7 +104,7 @@ class TestModifyPlanningScene(unittest.TestCase):
objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects]
self.assertTrue(objects == ["block", "box"])
def DISABLED_test_bw_remove_object(self):
def test_bw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box) backwards")
mps.removeObject("box")
self.task.insert(mps, 0)

View File

@ -231,10 +231,10 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros
if (trajectory())
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();
if (this->end()->scene()->getParent() == this->start()->scene())
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
else
this->end()->scene()->getPlanningSceneMsg(t.scene_diff);
}
double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {