mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
only attempt to merge if there is something to merge
This commit is contained in:
parent
51e1dda6ec
commit
433db67cf9
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user