Alternative fix

If start and end scene of a stage are identical (e.g. from a generator),
we can use an (empty) scene diff as well.
This commit is contained in:
Robert Haschke 2025-09-10 17:21:22 +02:00
parent 1bb80170ac
commit 86f29660f0

View File

@ -231,7 +231,11 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros
if (trajectory())
trajectory()->getRobotTrajectoryMsg(t.trajectory);
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
if (this->end()->scene()->getParent() == this->start()->scene() || // diff
this->end()->scene() == this->start()->scene()) // identical (from generator)
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 {