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: protected:
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories, SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes); 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 std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state); const moveit::core::RobotState& state);

View File

@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
// mark solution as failure // mark solution as failure
solution->setCost(std::numeric_limits<double>::infinity()); solution->setCost(std::numeric_limits<double>::infinity());
} else { } 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) { if (t) {
connect(from, to, SubTrajectory(t)); connect(from, to, SubTrajectory(t));
return true; return true;
@ -218,11 +218,16 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
return &solutions_.back(); 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 std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state) 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(); auto jmg = merged_jmg_.get();
assert(jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg); robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
// TODO: check merged trajectory for collisions // TODO: check merged trajectory for collisions
return trajectory; return trajectory;