diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index 1d6a97d1..7e93261a 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -63,7 +63,7 @@ bool findDuplicates(const std::vector& gro * or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory. * For now, only the trajectory path is considered. Timings, velocities, etc. are ignored. */ -robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, - core::JointModelGroup*& merged_group); +robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, + moveit::core::JointModelGroup*& merged_group); } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 238c9aaa..0c32ca38 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -197,7 +197,7 @@ protected: Stage(const Stage&) = delete; protected: - StagePrivate* const pimpl_; // constness guarantees one initial write + StagePrivate* pimpl_; }; std::ostream& operator<<(std::ostream& os, const Stage& stage); @@ -245,21 +245,6 @@ public: const InterfaceState& to, SubTrajectory&& trajectory); - void sendForward(const InterfaceState& from, - InterfaceState&& to, - SubTrajectory&& trajectory, - double cost) { - trajectory.setCost(cost); - sendForward(from, std::move(to), std::move(trajectory)); - } - void sendBackward(InterfaceState&& from, - const InterfaceState& to, - SubTrajectory&& trajectory, - double cost) { - trajectory.setCost(cost); - sendBackward(std::move(from), to, std::move(trajectory)); - } - protected: // constructor for use in derived classes PropagatingEitherWay(PropagatingEitherWayPrivate* impl); diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 8254598c..521f4079 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -70,9 +70,9 @@ public: void processFailures(const SolutionProcessor &processor) const override; protected: - SolutionBase* storeSequential(const std::vector& sub_trajectories, + SolutionBase* storeSequential(const std::vector& sub_trajectories, const std::vector& intermediate_scenes); - robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, + robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, const std::vector& intermediate_scenes); protected: diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 163c0017..d32101c9 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -242,8 +242,9 @@ private: /// SubTrajectory connects interface states of ComputeStages class SubTrajectory : public SolutionBase { public: - SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr()) - : SolutionBase(), trajectory_(trajectory) + SubTrajectory(const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(), + double cost = 0.0) + : SolutionBase(nullptr, cost), trajectory_(trajectory) {} robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } @@ -254,7 +255,7 @@ public: private: // actual trajectory, might be empty - robot_trajectory::RobotTrajectoryPtr trajectory_; + robot_trajectory::RobotTrajectoryConstPtr trajectory_; }; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 9642230e..04e754dc 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -64,12 +64,14 @@ MOVEIT_CLASS_FORWARD(Task) */ class Task : protected WrapperBase { public: - Task(const std::string& id = "", - Stage::pointer &&container = std::make_unique("task pipeline")); static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model, const std::string &ns = "move_group", const std::string &planning_plugin_param_name = "planning_plugin", const std::string &adapter_plugins_param_name = "request_adapters"); + Task(const std::string& id = "", + Stage::pointer &&container = std::make_unique("task pipeline")); + Task(Task &&other); + Task& operator=(Task&& other); ~Task(); std::string id() const; diff --git a/core/src/merge.cpp b/core/src/merge.cpp index 25ea3dee..39c58211 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -88,7 +88,7 @@ bool findDuplicates(const std::vector& gro return duplicates.size() > 0; } -robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, +robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, moveit::core::JointModelGroup*& merged_group) { if (sub_trajectories.size() <= 1) @@ -105,7 +105,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vectorgetRobotModel() != robot_model) throw std::runtime_error("subsolutions refer to multiple robot models"); @@ -153,7 +153,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vectorgetWayPointCount(); auto merged_state = index == 0 ? std::make_shared(robot_model) : std::make_shared(merged_traj->getLastWayPoint()); - for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) { + for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) { if (index >= sub->getWayPointCount()) continue; // no more waypoints in this sub solution diff --git a/core/src/stages/cartesian_position_motion.cpp b/core/src/stages/cartesian_position_motion.cpp index 596dfbdf..216bec7e 100644 --- a/core/src/stages/cartesian_position_motion.cpp +++ b/core/src/stages/cartesian_position_motion.cpp @@ -189,7 +189,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){ = std::make_shared(robot_state.getRobotModel(), jmg); for( auto& tp : trajectory_steps ) traj->addSuffixWayPoint(tp, 0.0); - sendForward(from, InterfaceState(result_scene), traj); + sendForward(from, InterfaceState(result_scene), SubTrajectory(traj)); } return succeeded; @@ -256,7 +256,7 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){ robot_trajectory::RobotTrajectoryPtr traj= std::make_shared(robot_state.getRobotModel(), jmg); for( auto& tp : trajectory_steps ) traj->addPrefixWayPoint(tp, 0.0); - sendBackward(InterfaceState(result_scene), to, traj); + sendBackward(InterfaceState(result_scene), to, SubTrajectory(traj)); } return succeeded; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index ad18b3dd..a6d7d7fd 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -141,7 +141,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { double timeout = props.get("timeout"); const auto& path_constraints = props.get("path_constraints"); - std::vector sub_trajectories; + std::vector sub_trajectories; planning_scene::PlanningScenePtr start = from.scene()->diff(); const moveit::core::RobotState& goal_state = to.scene()->getCurrentState(); @@ -188,7 +188,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { return !solution->isFailure(); } -SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, +SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, const std::vector& intermediate_scenes) { assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); @@ -217,7 +217,7 @@ SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, +robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, const std::vector& intermediate_scenes) { auto jmg = merged_jmg_.get(); diff --git a/core/src/stages/gripper.cpp b/core/src/stages/gripper.cpp index 58170982..1c039f80 100644 --- a/core/src/stages/gripper.cpp +++ b/core/src/stages/gripper.cpp @@ -145,7 +145,7 @@ bool Gripper::computeForward(const InterfaceState &from){ if (!compute(from, to, trajectory, cost, FORWARD)) return false; - sendForward(from, InterfaceState(to), trajectory, cost); + sendForward(from, InterfaceState(to), SubTrajectory(trajectory, cost)); return true; } @@ -157,7 +157,7 @@ bool Gripper::computeBackward(const InterfaceState &to) if (!compute(to, from, trajectory, cost, BACKWARD)) return false; - sendBackward(InterfaceState(from), to, trajectory, cost); + sendBackward(InterfaceState(from), to, SubTrajectory(trajectory, cost)); return true; } diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index c3aca100..a6bf8b97 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -65,13 +65,13 @@ void ModifyPlanningScene::enableCollisions(const std::string &first, const movei } bool ModifyPlanningScene::computeForward(const InterfaceState &from){ - sendForward(from, apply(from, false), robot_trajectory::RobotTrajectoryPtr()); + sendForward(from, apply(from, false), SubTrajectory()); return true; } bool ModifyPlanningScene::computeBackward(const InterfaceState &to) { - sendBackward(apply(to, true), to, robot_trajectory::RobotTrajectoryPtr()); + sendBackward(apply(to, true), to, SubTrajectory()); return true; } diff --git a/core/src/stages/move.cpp b/core/src/stages/move.cpp index 7096d6a9..7400128c 100644 --- a/core/src/stages/move.cpp +++ b/core/src/stages/move.cpp @@ -95,7 +95,7 @@ bool Move::compute(const InterfaceState &from, const InterfaceState &to) { return false; // finish stage - connect(from, to, res.trajectory_); + connect(from, to, SubTrajectory(res.trajectory_)); return true; } diff --git a/core/src/task.cpp b/core/src/task.cpp index a1719481..8cdee876 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -60,6 +60,22 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container) enableIntrospection(true); } +Task::Task(Task&& other) + : WrapperBase(std::string(), std::make_unique()) +{ + *this = std::move(other); +} + +Task& Task::operator=(Task&& other) +{ + id_ = std::move(other.id_); + robot_model_ = std::move(other.robot_model_); + introspection_ = std::move(other.introspection_); + task_cbs_ = std::move(other.task_cbs_); + std::swap(pimpl_, other.pimpl_); + return *this; +} + planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns, const std::string& planning_plugin_param_name, diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 00e4dd39..fe457f64 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -1,5 +1,6 @@ #include #include +#include #include "gtest_value_printers.h" #include @@ -450,3 +451,19 @@ TEST_F(ParallelTest, init_mismatching) { EXPECT_INIT_FAILURE(false, true, BOTH, GEN); EXPECT_INIT_FAILURE(true, true, BOTH, GEN); } + + +TEST(Task, move) { + Task t1("foo"); + t1.add(std::make_unique()); + t1.add(std::make_unique()); + EXPECT_EQ(t1.stages()->numChildren(), 2); + + Task t2 = std::move(t1); + EXPECT_EQ(t2.stages()->numChildren(), 2); + EXPECT_EQ(t1.stages()->numChildren(), 0); + + t1 = std::move(t2); + EXPECT_EQ(t1.stages()->numChildren(), 2); + EXPECT_EQ(t2.stages()->numChildren(), 0); +}