only attempt to merge if there is something to merge

This commit is contained in:
Robert Haschke 2018-04-15 11:04:04 +02:00
parent 51e1dda6ec
commit 433db67cf9
2 changed files with 12 additions and 7 deletions

View File

@ -76,7 +76,7 @@ public:
protected:
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state);

View File

@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
// mark solution as failure
solution->setCost(std::numeric_limits<double>::infinity());
} else {
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
if (t) {
connect(from, to, SubTrajectory(t));
return true;
@ -218,11 +218,16 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
return &solutions_.back();
}
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state)
{
// no need to merge if there is only a single sub trajectory
if (sub_trajectories.size() == 1)
return sub_trajectories[0];
auto jmg = merged_jmg_.get();
assert(jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
// TODO: check merged trajectory for collisions
return trajectory;