From 433db67cf9b1377674991267aa9de29cf558c579 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 15 Apr 2018 11:04:04 +0200 Subject: [PATCH] only attempt to merge if there is something to merge --- .../moveit/task_constructor/stages/connect.h | 6 +++--- core/src/stages/connect.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 083afcfc..35e4df2f 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -76,9 +76,9 @@ public: protected: SolutionBase* storeSequential(const std::vector& sub_trajectories, const std::vector& intermediate_scenes); - robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state); + robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state); protected: GroupPlannerVector planner_; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index beeb19b6..60a86a18 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { // mark solution as failure solution->setCost(std::numeric_limits::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& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state) +robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector& sub_trajectories, + const std::vector& 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;