mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
commit
795bde7b62
@ -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;
|
||||
};
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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 {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user