diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 67abd00b..ef24a9ee 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -187,7 +187,7 @@ protected: /// ComputeBase can only be instantiated by derived classes in stage.cpp ComputeBase(ComputeBasePrivate* impl); - SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost); + SubTrajectory& addTrajectory(SubTrajectory&& trajectory); }; @@ -206,12 +206,25 @@ public: void sendForward(const InterfaceState& from, InterfaceState&& to, - const robot_trajectory::RobotTrajectoryPtr& trajectory, - double cost = 0); + SubTrajectory&& trajectory); void sendBackward(InterfaceState&& from, const InterfaceState& to, - const robot_trajectory::RobotTrajectoryPtr& trajectory, - double cost = 0); + 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 @@ -254,7 +267,12 @@ public: virtual bool canCompute() const = 0; virtual bool compute() = 0; - void spawn(InterfaceState &&state, double cost = 0); + void spawn(InterfaceState &&state, SubTrajectory &&trajectory); + void spawn(InterfaceState &&state, double cost) { + SubTrajectory trajectory; + trajectory.setCost(cost); + spawn(std::move(state), std::move(trajectory)); + } }; @@ -266,7 +284,12 @@ public: virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0; void connect(const InterfaceState& from, const InterfaceState& to, - const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost = 0); + SubTrajectory&& trajectory); + void connect(const InterfaceState& from, const InterfaceState& to, + SubTrajectory&& trajectory, double cost) { + trajectory.setCost(cost); + connect(from, to, std::move(trajectory)); + } }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index f269b9cc..acaa3486 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -159,9 +159,6 @@ typedef std::vector SolutionTrajectory; class SolutionBase { public: - inline const StagePrivate* creator() const { return creator_; } - inline double cost() const { return cost_; } - inline const InterfaceState* start() const { return start_; } inline const InterfaceState* end() const { return end_; } @@ -176,14 +173,25 @@ public: end_ = &state; const_cast(state).addIncoming(this); } + + inline const StagePrivate* creator() const { return creator_; } + void setCreator(StagePrivate* creator); + + inline double cost() const { return cost_; } void setCost(double cost); + const std::string& name() const { return name_; } + void setName(const std::string& name) { name_ = name; } + + auto& markers() { return markers_; } + const auto& markers() const { return markers_; } + /// append this solution to Solution msg virtual void fillMessage(moveit_task_constructor_msgs::Solution &solution, Introspection* introspection = nullptr) const = 0; protected: - SolutionBase(StagePrivate* creator, double cost) + SolutionBase(StagePrivate* creator = nullptr, double cost = 0.0) : creator_(creator), cost_(cost) { } @@ -193,6 +201,10 @@ private: StagePrivate *creator_; // associated cost double cost_; + // trajectories could have a name, e.g. a generator could name its solutions + std::string name_; + // additional markers + std::deque markers_; // begin and end InterfaceState of this solution/trajectory const InterfaceState* start_ = nullptr; @@ -203,24 +215,18 @@ private: // SubTrajectory connects interface states of ComputeStages class SubTrajectory : public SolutionBase { public: - explicit SubTrajectory(StagePrivate* creator, const robot_trajectory::RobotTrajectoryPtr& traj, double cost); + SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr()) + : SolutionBase(), trajectory_(trajectory) + {} robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } - const auto& markers() const { return markers_; } - - const std::string& name() const { return name_; } - void setName(const std::string& name) { name_ = name; } void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection* introspection = nullptr) const override; private: - // trajectories could have a name, e.g. a generator could name its solutions - std::string name_; // actual trajectory, might be empty const robot_trajectory::RobotTrajectoryPtr trajectory_; - // additional markers - std::deque markers_; }; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ad1adbf2..5a7ebce9 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -43,11 +43,6 @@ namespace moveit { namespace task_constructor { -SubTrajectory::SubTrajectory(StagePrivate *creator, const robot_trajectory::RobotTrajectoryPtr &traj, double cost) - : SolutionBase(creator, cost), trajectory_(traj) -{} - - void InitStageException::push_back(const Stage &stage, const std::string &msg) { errors_.emplace_back(std::make_pair(&stage, msg)); @@ -217,9 +212,11 @@ ComputeBase::ComputeBase(ComputeBasePrivate *impl) { } -SubTrajectory& ComputeBase::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ - auto &trajs = pimpl()->trajectories_; - trajs.emplace_back(SubTrajectory(pimpl(), trajectory, cost)); +SubTrajectory& ComputeBase::addTrajectory(SubTrajectory&& trajectory){ + auto impl = pimpl(); + auto &trajs = impl->trajectories_; + trajectory.setCreator(impl); + trajs.emplace_back(trajectory); return trajs.back(); } @@ -400,11 +397,10 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, - const robot_trajectory::RobotTrajectoryPtr& t, - double cost){ + SubTrajectory&& t) { auto impl = pimpl(); std::cout << "sending state forward" << std::endl; - SubTrajectory &trajectory = addTrajectory(t, cost); + SubTrajectory &trajectory = addTrajectory(std::move(t)); trajectory.setStartState(from); impl->nextStarts()->add(std::move(to), &trajectory, NULL); impl->newSolution(trajectory); @@ -412,11 +408,10 @@ void PropagatingEitherWay::sendForward(const InterfaceState& from, void PropagatingEitherWay::sendBackward(InterfaceState&& from, const InterfaceState& to, - const robot_trajectory::RobotTrajectoryPtr& t, - double cost){ + SubTrajectory&& t) { auto impl = pimpl(); std::cout << "sending state backward" << std::endl; - SubTrajectory& trajectory = addTrajectory(t, cost); + SubTrajectory& trajectory = addTrajectory(std::move(t)); trajectory.setEndState(to); impl->prevEnds()->add(std::move(from), NULL, &trajectory); impl->newSolution(trajectory); @@ -487,16 +482,15 @@ Generator::Generator(const std::string &name) : ComputeBase(new GeneratorPrivate(this, name)) {} -void Generator::spawn(InterfaceState&& state, double cost) +void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) { std::cout << "spawning state forwards and backwards" << std::endl; assert(state.incomingTrajectories().empty() && state.outgoingTrajectories().empty()); + assert(!t.trajectory()); auto impl = pimpl(); - // empty trajectory ref -> this node only produces states - robot_trajectory::RobotTrajectoryPtr dummy; - SubTrajectory& trajectory = addTrajectory(dummy, cost); + SubTrajectory& trajectory = addTrajectory(std::move(t)); impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory); impl->nextStarts()->add(std::move(state), &trajectory, NULL); impl->newSolution(trajectory); @@ -554,9 +548,9 @@ Connecting::Connecting(const std::string &name) } void Connecting::connect(const InterfaceState& from, const InterfaceState& to, - const robot_trajectory::RobotTrajectoryPtr& t, double cost) { + SubTrajectory&& t) { auto impl = pimpl(); - SubTrajectory& trajectory = addTrajectory(t, cost); + SubTrajectory& trajectory = addTrajectory(std::move(t)); trajectory.setStartState(from); trajectory.setEndState(to); impl->newSolution(trajectory); diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index fd5c26d1..f2bd0125 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -58,7 +58,7 @@ bool CurrentState::canCompute() const{ bool CurrentState::compute(){ ran_= true; - spawn(InterfaceState(scene_)); + spawn(InterfaceState(scene_), 0.0); return true; } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 9f139612..62660280 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -234,7 +234,7 @@ bool GenerateGraspPose::compute(){ if(succeeded) { previous_solutions_.emplace_back(); grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back()); - spawn(InterfaceState(grasp_scene)); + spawn(InterfaceState(grasp_scene), 0.0); return true; } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index e646f85b..629d7eec 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -41,6 +41,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -84,11 +85,19 @@ Interface::iterator Interface::clone(const InterfaceState &state) } +void SolutionBase::setCreator(StagePrivate *creator) +{ + assert(creator_ == nullptr || creator_ == creator); // creator must only set once + creator_ = creator; +} + void SolutionBase::setCost(double cost) { cost_ = cost; } + + void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const { msg.sub_trajectory.emplace_back();