allow stages to access the created SubTrajectory

to add markers, set name, etc.
This commit is contained in:
Robert Haschke 2017-11-29 11:21:49 +01:00
parent 30f6ade636
commit 0e8dd85ae6
6 changed files with 74 additions and 42 deletions

View File

@ -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));
}
};

View File

@ -159,9 +159,6 @@ typedef std::vector<const SubTrajectory*> 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<InterfaceState&>(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<visualization_msgs::Marker> 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<visualization_msgs::Marker> markers_;
};

View File

@ -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);

View File

@ -58,7 +58,7 @@ bool CurrentState::canCompute() const{
bool CurrentState::compute(){
ran_= true;
spawn(InterfaceState(scene_));
spawn(InterfaceState(scene_), 0.0);
return true;
}

View File

@ -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;
}
}

View File

@ -41,6 +41,7 @@
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/planning_scene/planning_scene.h>
#include <assert.h>
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();