mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
simplified SubTask API
- moved interface flags to private implementation - moved newInputState() / newOutputState to private implementation - directly implement sendBackward()/sendForward() - removed scene_, planner_ from SubTaskPrivate ... replaced by init(scene) call - renamings - array_type -> container_type - ...AnyWay -> ...EitherWay - input -> start - output -> end
This commit is contained in:
parent
c085534ea9
commit
1b1a82f7c8
@ -30,6 +30,7 @@ add_library(${PROJECT_NAME}
|
|||||||
src/debug.cpp
|
src/debug.cpp
|
||||||
|
|
||||||
src/subtask_p.h
|
src/subtask_p.h
|
||||||
|
src/container_p.h
|
||||||
include/${PROJECT_NAME}/utils.h
|
include/${PROJECT_NAME}/utils.h
|
||||||
include/${PROJECT_NAME}/storage.h
|
include/${PROJECT_NAME}/storage.h
|
||||||
include/${PROJECT_NAME}/subtask.h
|
include/${PROJECT_NAME}/subtask.h
|
||||||
|
|||||||
@ -18,6 +18,10 @@ public:
|
|||||||
virtual bool insert(value_type&& stage, int before = -1) = 0;
|
virtual bool insert(value_type&& stage, int before = -1) = 0;
|
||||||
virtual void clear();
|
virtual void clear();
|
||||||
|
|
||||||
|
bool init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||||
|
bool canCompute() const;
|
||||||
|
bool compute();
|
||||||
|
|
||||||
bool traverseStages(const StageCallback &processor) const;
|
bool traverseStages(const StageCallback &processor) const;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
@ -34,9 +38,6 @@ public:
|
|||||||
bool canInsert(const value_type& stage, int before = -1) const override;
|
bool canInsert(const value_type& stage, int before = -1) const override;
|
||||||
bool insert(value_type&& stage, int before = -1) override;
|
bool insert(value_type&& stage, int before = -1) override;
|
||||||
|
|
||||||
bool canCompute() const override;
|
|
||||||
bool compute() override;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
SerialContainer(SerialContainerPrivate* impl);
|
SerialContainer(SerialContainerPrivate* impl);
|
||||||
};
|
};
|
||||||
|
|||||||
@ -42,41 +42,17 @@ public:
|
|||||||
inline SubTaskPrivate* pimpl_func() { return pimpl_; }
|
inline SubTaskPrivate* pimpl_func() { return pimpl_; }
|
||||||
|
|
||||||
typedef std::unique_ptr<SubTask> pointer;
|
typedef std::unique_ptr<SubTask> pointer;
|
||||||
enum InterfaceFlag {
|
|
||||||
READS_INPUT = 0x01,
|
|
||||||
READS_OUTPUT = 0x02,
|
|
||||||
WRITES_NEXT_INPUT = 0x04,
|
|
||||||
WRITES_PREV_OUTPUT = 0x08,
|
|
||||||
|
|
||||||
OWN_IF_MASK = READS_INPUT | READS_OUTPUT,
|
|
||||||
EXT_IF_MASK = WRITES_NEXT_INPUT | WRITES_PREV_OUTPUT,
|
|
||||||
INPUT_IF_MASK = READS_INPUT | WRITES_PREV_OUTPUT,
|
|
||||||
OUTPUT_IF_MASK = READS_OUTPUT | WRITES_NEXT_INPUT,
|
|
||||||
};
|
|
||||||
typedef Flags<InterfaceFlag> InterfaceFlags;
|
|
||||||
InterfaceFlags interfaceFlags() const;
|
|
||||||
|
|
||||||
~SubTask();
|
~SubTask();
|
||||||
|
|
||||||
|
/// initialize stage once before planning
|
||||||
|
virtual bool init(const planning_scene::PlanningSceneConstPtr& scene);
|
||||||
const std::string& getName() const;
|
const std::string& getName() const;
|
||||||
|
|
||||||
|
|
||||||
// TODO: results from { TIMEOUT, EXHAUSTED, FINISHED, WAITING }
|
|
||||||
virtual bool canCompute() const = 0;
|
|
||||||
virtual bool compute() = 0;
|
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr scene() const;
|
|
||||||
planning_pipeline::PlanningPipelinePtr planner() const;
|
|
||||||
void setPlanningScene(const planning_scene::PlanningSceneConstPtr&);
|
|
||||||
void setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr&);
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/// can only instantiated by derived classes
|
/// SubTask can only be instantiated through derived classes
|
||||||
SubTask(SubTaskPrivate *impl);
|
SubTask(SubTaskPrivate *impl);
|
||||||
|
|
||||||
/// methods called when a new InterfaceState was spawned
|
|
||||||
virtual void newInputState(const std::list<InterfaceState>::iterator&) {}
|
|
||||||
virtual void newOutputState(const std::list<InterfaceState>::iterator&) {}
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor
|
// TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor
|
||||||
SubTaskPrivate* const pimpl_; // constness guarantees one initial write
|
SubTaskPrivate* const pimpl_; // constness guarantees one initial write
|
||||||
@ -84,55 +60,57 @@ protected:
|
|||||||
std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
||||||
|
|
||||||
|
|
||||||
class PropagatingAnyWayPrivate;
|
class PropagatingEitherWayPrivate;
|
||||||
class PropagatingAnyWay : public SubTask {
|
class PropagatingEitherWay : public SubTask {
|
||||||
public:
|
public:
|
||||||
PRIVATE_CLASS(PropagatingAnyWay)
|
PRIVATE_CLASS(PropagatingEitherWay)
|
||||||
PropagatingAnyWay(const std::string& name);
|
PropagatingEitherWay(const std::string& name);
|
||||||
|
|
||||||
enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
|
enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
|
||||||
void restrictDirection(Direction dir);
|
void restrictDirection(Direction dir);
|
||||||
|
|
||||||
virtual bool computeForward(const InterfaceState& from, planning_scene::PlanningScenePtr& to,
|
virtual bool computeForward(const InterfaceState& from) = 0;
|
||||||
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost);
|
virtual bool computeBackward(const InterfaceState& to) = 0;
|
||||||
virtual bool computeBackward(planning_scene::PlanningScenePtr& from, const InterfaceState& to,
|
|
||||||
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost);
|
|
||||||
|
|
||||||
bool canCompute() const override;
|
void sendForward(const InterfaceState& from,
|
||||||
bool compute() override;
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
|
const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
||||||
|
double cost = 0);
|
||||||
|
void sendBackward(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
|
const InterfaceState& to,
|
||||||
|
const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
||||||
|
double cost = 0);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// constructor for use in derived classes
|
// constructor for use in derived classes
|
||||||
PropagatingAnyWay(PropagatingAnyWayPrivate* impl);
|
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
|
||||||
void initInterface();
|
void initInterface();
|
||||||
|
|
||||||
// get informed when new beginnings and endings become available
|
|
||||||
void newInputState(const std::list<InterfaceState>::iterator& it);
|
|
||||||
void newOutputState(const std::list<InterfaceState>::iterator& it);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
class PropagatingForwardPrivate;
|
class PropagatingForwardPrivate;
|
||||||
class PropagatingForward : public PropagatingAnyWay {
|
class PropagatingForward : public PropagatingEitherWay {
|
||||||
public:
|
public:
|
||||||
PRIVATE_CLASS(PropagatingForward)
|
PRIVATE_CLASS(PropagatingForward)
|
||||||
PropagatingForward(const std::string& name);
|
PropagatingForward(const std::string& name);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// restrict access to backward method to provide compile-time check
|
// restrict access to backward method to provide compile-time check
|
||||||
using PropagatingAnyWay::computeBackward;
|
bool computeBackward(const InterfaceState &to) override;
|
||||||
|
using PropagatingEitherWay::sendBackward;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
class PropagatingBackwardPrivate;
|
class PropagatingBackwardPrivate;
|
||||||
class PropagatingBackward : public PropagatingAnyWay {
|
class PropagatingBackward : public PropagatingEitherWay {
|
||||||
public:
|
public:
|
||||||
PRIVATE_CLASS(PropagatingBackward)
|
PRIVATE_CLASS(PropagatingBackward)
|
||||||
PropagatingBackward(const std::string& name);
|
PropagatingBackward(const std::string& name);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// restrict access to forward method to provide compile-time check
|
// restrict access to forward method to provide compile-time check
|
||||||
using PropagatingAnyWay::computeForward;
|
bool computeForward(const InterfaceState &from) override;
|
||||||
|
using PropagatingEitherWay::sendForward;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -142,7 +120,9 @@ public:
|
|||||||
PRIVATE_CLASS(Generator)
|
PRIVATE_CLASS(Generator)
|
||||||
Generator(const std::string& name);
|
Generator(const std::string& name);
|
||||||
|
|
||||||
bool spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
|
virtual bool canCompute() const = 0;
|
||||||
|
virtual bool compute() = 0;
|
||||||
|
void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -152,15 +132,9 @@ public:
|
|||||||
PRIVATE_CLASS(Connecting)
|
PRIVATE_CLASS(Connecting)
|
||||||
Connecting(const std::string& name);
|
Connecting(const std::string& name);
|
||||||
|
|
||||||
// methods for manual use
|
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
|
||||||
bool hasStatePair() const;
|
void connect(const InterfaceState& from, const InterfaceState& to,
|
||||||
InterfaceStatePair fetchStatePair();
|
const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost = 0);
|
||||||
void connect(const robot_trajectory::RobotTrajectoryPtr&,
|
|
||||||
const InterfaceStatePair&, double cost = 0);
|
|
||||||
|
|
||||||
protected:
|
|
||||||
virtual void newInputState(const std::list<InterfaceState>::iterator& it);
|
|
||||||
virtual void newOutputState(const std::list<InterfaceState>::iterator& it);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -16,14 +16,12 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
class CartesianPositionMotion : public PropagatingAnyWay {
|
class CartesianPositionMotion : public PropagatingEitherWay {
|
||||||
public:
|
public:
|
||||||
CartesianPositionMotion(std::string name);
|
CartesianPositionMotion(std::string name);
|
||||||
|
|
||||||
virtual bool computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to,
|
virtual bool computeForward(const InterfaceState &from) override;
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) override;
|
virtual bool computeBackward(const InterfaceState &to) override;
|
||||||
virtual bool computeBackward(planning_scene::PlanningScenePtr &from, const InterfaceState &to,
|
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) override;
|
|
||||||
|
|
||||||
void setGroup(std::string group);
|
void setGroup(std::string group);
|
||||||
void setLink(std::string link);
|
void setLink(std::string link);
|
||||||
@ -57,7 +55,7 @@ protected:
|
|||||||
|
|
||||||
ros::Publisher pub;
|
ros::Publisher pub;
|
||||||
|
|
||||||
void _publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start);
|
void _publishTrajectory(const planning_scene::PlanningSceneConstPtr &scene, const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start);
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -10,10 +10,12 @@ class CurrentState : public Generator {
|
|||||||
public:
|
public:
|
||||||
CurrentState(std::string name);
|
CurrentState(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute() const override;
|
bool init(const planning_scene::PlanningSceneConstPtr& scene) override;
|
||||||
virtual bool compute() override;
|
bool canCompute() const override;
|
||||||
|
bool compute() override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
bool ran_;
|
bool ran_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -18,8 +18,9 @@ class GenerateGraspPose : public Generator {
|
|||||||
public:
|
public:
|
||||||
GenerateGraspPose(std::string name);
|
GenerateGraspPose(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute() const override;
|
bool init(const planning_scene::PlanningSceneConstPtr& scene) override;
|
||||||
virtual bool compute() override;
|
bool canCompute() const override;
|
||||||
|
bool compute() override;
|
||||||
|
|
||||||
void setEndEffector(std::string eef);
|
void setEndEffector(std::string eef);
|
||||||
|
|
||||||
@ -42,6 +43,8 @@ public:
|
|||||||
void ignoreCollisions(bool flag);
|
void ignoreCollisions(bool flag);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
|
|
||||||
std::string eef_;
|
std::string eef_;
|
||||||
|
|
||||||
std::string group_;
|
std::string group_;
|
||||||
|
|||||||
@ -10,16 +10,13 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
class Gripper : public PropagatingAnyWay {
|
class Gripper : public PropagatingEitherWay {
|
||||||
public:
|
public:
|
||||||
Gripper(std::string name);
|
Gripper(std::string name);
|
||||||
|
|
||||||
bool compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
|
bool init(const planning_scene::PlanningSceneConstPtr &scene);
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
|
bool computeForward(const InterfaceState& from) override;
|
||||||
bool computeForward(const InterfaceState& from, planning_scene::PlanningScenePtr &to,
|
bool computeBackward(const InterfaceState& to) override;
|
||||||
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost) override;
|
|
||||||
bool computeBackward(planning_scene::PlanningScenePtr& from, const InterfaceState& to,
|
|
||||||
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost) override;
|
|
||||||
|
|
||||||
void setEndEffector(std::string eef);
|
void setEndEffector(std::string eef);
|
||||||
void setAttachLink(std::string link);
|
void setAttachLink(std::string link);
|
||||||
@ -29,12 +26,17 @@ public:
|
|||||||
|
|
||||||
void graspObject(std::string grasp_object);
|
void graspObject(std::string grasp_object);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
bool compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
|
||||||
|
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::string eef_;
|
std::string eef_;
|
||||||
std::string named_target_;
|
std::string named_target_;
|
||||||
std::string grasp_object_;
|
std::string grasp_object_;
|
||||||
std::string attach_link_;
|
std::string attach_link_;
|
||||||
|
|
||||||
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -14,29 +14,19 @@ class Move : public Connecting {
|
|||||||
public:
|
public:
|
||||||
Move(std::string name);
|
Move(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute() const override;
|
bool init(const planning_scene::PlanningSceneConstPtr &scene);
|
||||||
virtual bool compute() override;
|
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||||
|
|
||||||
void setGroup(std::string group);
|
void setGroup(std::string group);
|
||||||
void setLink(std::string link);
|
|
||||||
|
|
||||||
void setPlannerId(std::string planner);
|
void setPlannerId(std::string planner);
|
||||||
void setTimeout(double timeout);
|
void setTimeout(double timeout);
|
||||||
|
|
||||||
void setFrom(std::string named_target);
|
|
||||||
void setTo(std::string named_target);
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::string group_;
|
std::string group_;
|
||||||
std::string link_;
|
std::string planner_id_;
|
||||||
|
|
||||||
double timeout_;
|
double timeout_;
|
||||||
|
|
||||||
std::string planner_id_;
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
|
|
||||||
std::string from_named_target_;
|
|
||||||
std::string to_named_target_;
|
|
||||||
|
|
||||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
// copyright Michael 'v4hn' Goerner @ 2017
|
// copyright Michael 'v4hn' Goerner @ 2017
|
||||||
|
// copyright Robert Haschke @ 2017
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
@ -6,11 +7,8 @@
|
|||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace moveit { namespace core {
|
namespace moveit { namespace core {
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState)
|
||||||
}}
|
}}
|
||||||
|
|
||||||
@ -24,9 +22,11 @@ class TaskPrivate;
|
|||||||
class Task : protected SerialContainer {
|
class Task : protected SerialContainer {
|
||||||
PRIVATE_CLASS(Task)
|
PRIVATE_CLASS(Task)
|
||||||
public:
|
public:
|
||||||
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
|
|
||||||
|
|
||||||
Task(const std::string &name = std::string());
|
Task(const std::string &name = std::string());
|
||||||
|
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");
|
||||||
|
|
||||||
void add(SubTask::pointer &&stage);
|
void add(SubTask::pointer &&stage);
|
||||||
using SerialContainer::clear;
|
using SerialContainer::clear;
|
||||||
@ -35,6 +35,8 @@ public:
|
|||||||
void printState();
|
void printState();
|
||||||
|
|
||||||
const moveit::core::RobotState &getCurrentRobotState() const;
|
const moveit::core::RobotState &getCurrentRobotState() const;
|
||||||
|
|
||||||
|
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
|
||||||
bool processSolutions(const SolutionCallback &processor);
|
bool processSolutions(const SolutionCallback &processor);
|
||||||
bool processSolutions(const SolutionCallback &processor) const;
|
bool processSolutions(const SolutionCallback &processor) const;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -11,7 +11,7 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
|
|||||||
for (auto end = children_.end(); before > 0 && position != end; --before)
|
for (auto end = children_.end(); before > 0 && position != end; --before)
|
||||||
++position;
|
++position;
|
||||||
} else if (++before <= 0) {
|
} else if (++before <= 0) {
|
||||||
array_type::const_reverse_iterator from_end = children_.rbegin();
|
container_type::const_reverse_iterator from_end = children_.rbegin();
|
||||||
for (auto end = children_.rend(); before < 0 && from_end != end; ++before)
|
for (auto end = children_.rend(); before < 0 && from_end != end; ++before)
|
||||||
++from_end;
|
++from_end;
|
||||||
position = from_end.base();
|
position = from_end.base();
|
||||||
@ -29,9 +29,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
|
|||||||
for (auto &stage : children_) {
|
for (auto &stage : children_) {
|
||||||
if (!processor(*stage, depth))
|
if (!processor(*stage, depth))
|
||||||
continue;
|
continue;
|
||||||
ContainerBase *container = dynamic_cast<ContainerBase*>(stage.get());
|
ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl_func());
|
||||||
if (container)
|
if (container)
|
||||||
static_cast<ContainerBasePrivate*>(container->pimpl_func())->traverseStages(processor, depth+1);
|
container->traverseStages(processor, depth+1);
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -40,8 +40,6 @@ ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate
|
|||||||
ContainerBasePrivate::const_iterator pos) {
|
ContainerBasePrivate::const_iterator pos) {
|
||||||
SubTaskPrivate *impl = subtask->pimpl_func();
|
SubTaskPrivate *impl = subtask->pimpl_func();
|
||||||
impl->parent_ = this;
|
impl->parent_ = this;
|
||||||
subtask->setPlanningScene(scene_);
|
|
||||||
subtask->setPlanningPipeline(planner_);
|
|
||||||
impl->it_ = children_.insert(pos, std::move(subtask));
|
impl->it_ = children_.insert(pos, std::move(subtask));
|
||||||
return impl->it_;
|
return impl->it_;
|
||||||
}
|
}
|
||||||
@ -58,28 +56,46 @@ void ContainerBase::clear()
|
|||||||
impl->clear();
|
impl->clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
IMPL(ContainerBase);
|
||||||
|
for (auto& stage : impl->children_)
|
||||||
|
stage->init(scene);
|
||||||
|
}
|
||||||
|
|
||||||
bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
|
bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
|
||||||
{
|
{
|
||||||
IMPL(const ContainerBase);
|
IMPL(const ContainerBase);
|
||||||
return impl->traverseStages(processor, 0);
|
return impl->traverseStages(processor, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool ContainerBase::canCompute() const
|
||||||
|
{
|
||||||
|
IMPL(const ContainerBase);
|
||||||
|
return impl->canCompute();
|
||||||
|
}
|
||||||
|
|
||||||
SubTask::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
bool ContainerBase::compute() {
|
||||||
SubTask::InterfaceFlags f;
|
IMPL(ContainerBase);
|
||||||
|
return impl->compute();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
||||||
|
InterfaceFlags f;
|
||||||
if (children().empty()) return f;
|
if (children().empty()) return f;
|
||||||
f |= children().front()->pimpl_func()->announcedFlags() & SubTask::INPUT_IF_MASK;
|
f |= children().front()->pimpl_func()->announcedFlags() & INPUT_IF_MASK;
|
||||||
f |= children().back()->pimpl_func()->announcedFlags() & SubTask::OUTPUT_IF_MASK;
|
f |= children().back()->pimpl_func()->announcedFlags() & OUTPUT_IF_MASK;
|
||||||
return f;
|
return f;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool isConnectable(int prev_flags, int next_flags) {
|
inline bool isConnectable(int prev_flags, int next_flags) {
|
||||||
return ((prev_flags & SubTask::WRITES_NEXT_INPUT) && (next_flags & SubTask::READS_INPUT)) ||
|
return ((prev_flags & SubTaskPrivate::WRITES_NEXT_START) && (next_flags & SubTaskPrivate::READS_START)) ||
|
||||||
((prev_flags & SubTask::READS_OUTPUT) && (next_flags & SubTask::WRITES_PREV_OUTPUT));
|
((prev_flags & SubTaskPrivate::READS_END) && (next_flags & SubTaskPrivate::WRITES_PREV_END));
|
||||||
}
|
}
|
||||||
inline bool bothWrite(SubTask::InterfaceFlags prev_flags, SubTask::InterfaceFlags next_flags) {
|
inline bool bothWrite(SubTaskPrivate::InterfaceFlags prev_flags, SubTaskPrivate::InterfaceFlags next_flags) {
|
||||||
return (prev_flags.testFlag(SubTask::WRITES_NEXT_INPUT) && !next_flags.testFlag(SubTask::READS_INPUT)) &&
|
return (prev_flags.testFlag(SubTaskPrivate::WRITES_NEXT_START) && !next_flags.testFlag(SubTaskPrivate::READS_START)) &&
|
||||||
(next_flags.testFlag(SubTask::WRITES_PREV_OUTPUT) && !prev_flags.testFlag(SubTask::READS_OUTPUT));
|
(next_flags.testFlag(SubTaskPrivate::WRITES_PREV_END) && !prev_flags.testFlag(SubTaskPrivate::READS_END));
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBasePrivate::const_iterator before) const {
|
inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBasePrivate::const_iterator before) const {
|
||||||
@ -89,9 +105,9 @@ inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBas
|
|||||||
// check connectedness
|
// check connectedness
|
||||||
bool at_end = (before == children().end());
|
bool at_end = (before == children().end());
|
||||||
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl_func();
|
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl_func();
|
||||||
SubTask::InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags();
|
InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags();
|
||||||
SubTask::InterfaceFlags next_flags = next->deducedFlags();
|
InterfaceFlags next_flags = next->deducedFlags();
|
||||||
SubTask::InterfaceFlags prev_flags = prev(before)->deducedFlags();
|
InterfaceFlags prev_flags = prev(before)->deducedFlags();
|
||||||
|
|
||||||
// Do a simple check here only. A full connectivity check requires the full pipeline to be setup
|
// Do a simple check here only. A full connectivity check requires the full pipeline to be setup
|
||||||
// Thus, here we reject when trying to connect to writers with each other
|
// Thus, here we reject when trying to connect to writers with each other
|
||||||
@ -108,27 +124,27 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
|
|||||||
bool at_end = (before == children().end());
|
bool at_end = (before == children().end());
|
||||||
|
|
||||||
SubTaskPrivate *cur = stage->pimpl_func();
|
SubTaskPrivate *cur = stage->pimpl_func();
|
||||||
/* set pointer cache (prev_ouput_ and next_input_) of prev, current, and next stage */
|
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
|
||||||
if (children().empty()) { // first child inserted
|
if (children().empty()) { // first child inserted
|
||||||
setPrevOutput(cur, this->input_);
|
setPrevEnds(cur, this->starts_);
|
||||||
setNextInput(cur, this->output_);
|
setNextStarts(cur, this->ends_);
|
||||||
} else if (at_begin) {
|
} else if (at_begin) {
|
||||||
SubTaskPrivate *next = (*before)->pimpl_func();
|
SubTaskPrivate *next = (*before)->pimpl_func();
|
||||||
setPrevOutput(cur, this->input_);
|
setPrevEnds(cur, this->starts_);
|
||||||
setNextInput(cur, next->input_);
|
setNextStarts(cur, next->starts_);
|
||||||
setPrevOutput(next, cur->output_);
|
setPrevEnds(next, cur->ends_);
|
||||||
} else if (at_end) {
|
} else if (at_end) {
|
||||||
const SubTaskPrivate *prev = this->prev(before);
|
const SubTaskPrivate *prev = this->prev(before);
|
||||||
setNextInput(prev, cur->input_);
|
setNextStarts(prev, cur->starts_);
|
||||||
setPrevOutput(cur, prev->output_);
|
setPrevEnds(cur, prev->ends_);
|
||||||
setNextInput(cur, this->output_);
|
setNextStarts(cur, this->ends_);
|
||||||
} else {
|
} else {
|
||||||
const SubTaskPrivate *prev = this->prev(before);
|
const SubTaskPrivate *prev = this->prev(before);
|
||||||
SubTaskPrivate *next = (*before)->pimpl_func();
|
SubTaskPrivate *next = (*before)->pimpl_func();
|
||||||
setNextInput(prev, cur->input_);
|
setNextStarts(prev, cur->starts_);
|
||||||
setPrevOutput(cur, prev->output_);
|
setPrevEnds(cur, prev->ends_);
|
||||||
setNextInput(cur, next->input_);
|
setNextStarts(cur, next->starts_);
|
||||||
setPrevOutput(next, cur->output_);
|
setPrevEnds(next, cur->ends_);
|
||||||
}
|
}
|
||||||
|
|
||||||
iterator it = ContainerBasePrivate::insert(std::move(stage), before);
|
iterator it = ContainerBasePrivate::insert(std::move(stage), before);
|
||||||
@ -195,21 +211,19 @@ bool SerialContainer::insert(value_type&& subtask, int before)
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SerialContainer::canCompute() const
|
bool SerialContainerPrivate::canCompute() const
|
||||||
{
|
{
|
||||||
IMPL(const SerialContainer);
|
return children().size() > 0;
|
||||||
return impl->children().size() > 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SerialContainer::compute()
|
bool SerialContainerPrivate::compute()
|
||||||
{
|
{
|
||||||
IMPL(SerialContainer);
|
|
||||||
bool computed = false;
|
bool computed = false;
|
||||||
for(const auto& stage : impl->children()){
|
for(const auto& stage : children()) {
|
||||||
if(!stage->canCompute())
|
if(!stage->pimpl_func()->canCompute())
|
||||||
continue;
|
continue;
|
||||||
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;
|
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;
|
||||||
bool success = stage->compute();
|
bool success = stage->pimpl_func()->compute();
|
||||||
computed = true;
|
computed = true;
|
||||||
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -7,14 +7,16 @@ namespace moveit { namespace task_constructor {
|
|||||||
|
|
||||||
class ContainerBasePrivate : public SubTaskPrivate
|
class ContainerBasePrivate : public SubTaskPrivate
|
||||||
{
|
{
|
||||||
|
friend class ContainerBase;
|
||||||
friend class BaseTest; // allow access for unit tests
|
friend class BaseTest; // allow access for unit tests
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef ContainerBase::value_type value_type;
|
typedef ContainerBase::value_type value_type;
|
||||||
typedef SubTaskPrivate::array_type array_type;
|
typedef SubTaskPrivate::container_type container_type;
|
||||||
typedef array_type::iterator iterator;
|
typedef container_type::iterator iterator;
|
||||||
typedef array_type::const_iterator const_iterator;
|
typedef container_type::const_iterator const_iterator;
|
||||||
|
|
||||||
inline const array_type& children() const { return children_; }
|
inline const container_type& children() const { return children_; }
|
||||||
const_iterator position(int before = -1) const;
|
const_iterator position(int before = -1) const;
|
||||||
|
|
||||||
bool canInsert(const SubTask& stage) const;
|
bool canInsert(const SubTask& stage) const;
|
||||||
@ -30,15 +32,15 @@ protected:
|
|||||||
inline const ContainerBasePrivate* parent(const SubTaskPrivate *child) const { return child->parent_; }
|
inline const ContainerBasePrivate* parent(const SubTaskPrivate *child) const { return child->parent_; }
|
||||||
inline iterator it(const SubTaskPrivate *child) const { return child->it_; }
|
inline iterator it(const SubTaskPrivate *child) const { return child->it_; }
|
||||||
|
|
||||||
inline void setPrevOutput(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
|
inline void setPrevEnds(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
|
||||||
child->prev_output_ = interface.get();
|
child->prev_ends_ = interface.get();
|
||||||
}
|
}
|
||||||
inline void setNextInput(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
|
inline void setNextStarts(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
|
||||||
child->next_input_ = interface.get();
|
child->next_starts_ = interface.get();
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
array_type children_;
|
container_type children_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -47,14 +49,17 @@ public:
|
|||||||
SerialContainerPrivate(SerialContainer* me, const std::string &name)
|
SerialContainerPrivate(SerialContainer* me, const std::string &name)
|
||||||
: ContainerBasePrivate(me, name)
|
: ContainerBasePrivate(me, name)
|
||||||
{
|
{
|
||||||
input_.reset(new Interface(Interface::NotifyFunction()));
|
starts_.reset(new Interface(Interface::NotifyFunction()));
|
||||||
output_.reset(new Interface(Interface::NotifyFunction()));
|
ends_.reset(new Interface(Interface::NotifyFunction()));
|
||||||
}
|
}
|
||||||
|
|
||||||
SubTask::InterfaceFlags announcedFlags() const override;
|
InterfaceFlags announcedFlags() const override;
|
||||||
bool canInsert(const SubTask& stage, const_iterator before) const;
|
bool canInsert(const SubTask& stage, const_iterator before) const;
|
||||||
virtual iterator insert(value_type &&stage, const_iterator before) override;
|
virtual iterator insert(value_type &&stage, const_iterator before) override;
|
||||||
|
|
||||||
|
bool canCompute() const override;
|
||||||
|
bool compute() override;
|
||||||
|
|
||||||
inline const SubTaskPrivate *prev(const_iterator it) const;
|
inline const SubTaskPrivate *prev(const_iterator it) const;
|
||||||
inline const SubTaskPrivate *next(const_iterator it) const;
|
inline const SubTaskPrivate *next(const_iterator it) const;
|
||||||
|
|
||||||
|
|||||||
@ -55,7 +55,6 @@ int main(int argc, char** argv){
|
|||||||
{
|
{
|
||||||
auto move= std::make_unique<subtasks::Move>("move to pre-grasp");
|
auto move= std::make_unique<subtasks::Move>("move to pre-grasp");
|
||||||
move->setGroup("left_arm");
|
move->setGroup("left_arm");
|
||||||
move->setLink("l_gripper_tool_frame");
|
|
||||||
move->setPlannerId("RRTConnectkConfigDefault");
|
move->setPlannerId("RRTConnectkConfigDefault");
|
||||||
move->setTimeout(8.0);
|
move->setTimeout(8.0);
|
||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
|
|||||||
445
src/subtask.cpp
445
src/subtask.cpp
@ -11,19 +11,15 @@ SubTask::SubTask(SubTaskPrivate *impl)
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
SubTask::InterfaceFlags SubTask::interfaceFlags() const
|
|
||||||
{
|
|
||||||
SubTask::InterfaceFlags result = pimpl_->announcedFlags();
|
|
||||||
result &= ~SubTask::InterfaceFlags(SubTask::OWN_IF_MASK);
|
|
||||||
result |= pimpl_->deducedFlags();
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
SubTask::~SubTask()
|
SubTask::~SubTask()
|
||||||
{
|
{
|
||||||
delete pimpl_;
|
delete pimpl_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool SubTask::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
const std::string& SubTask::getName() const {
|
const std::string& SubTask::getName() const {
|
||||||
return pimpl_->name_;
|
return pimpl_->name_;
|
||||||
}
|
}
|
||||||
@ -33,12 +29,12 @@ std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
|
|||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<SubTask::InterfaceFlag own, SubTask::InterfaceFlag other>
|
template<SubTaskPrivate::InterfaceFlag own, SubTaskPrivate::InterfaceFlag other>
|
||||||
const char* direction(const SubTaskPrivate& stage) {
|
const char* direction(const SubTaskPrivate& stage) {
|
||||||
SubTask::InterfaceFlags f = stage.deducedFlags();
|
SubTaskPrivate::InterfaceFlags f = stage.deducedFlags();
|
||||||
bool own_if = f & own;
|
bool own_if = f & own;
|
||||||
bool other_if = f & other;
|
bool other_if = f & other;
|
||||||
bool reverse = own & SubTask::INPUT_IF_MASK;
|
bool reverse = own & SubTaskPrivate::INPUT_IF_MASK;
|
||||||
if (own_if && other_if) return "<>";
|
if (own_if && other_if) return "<>";
|
||||||
if (!own_if && !other_if) return "--";
|
if (!own_if && !other_if) return "--";
|
||||||
if (other_if ^ reverse) return "->";
|
if (other_if ^ reverse) return "->";
|
||||||
@ -46,18 +42,18 @@ const char* direction(const SubTaskPrivate& stage) {
|
|||||||
};
|
};
|
||||||
|
|
||||||
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||||
// inputs
|
// starts
|
||||||
for (const Interface* i : {stage.prev_output_, stage.input_.get()}) {
|
for (const Interface* i : {stage.prev_ends_, stage.starts_.get()}) {
|
||||||
os << std::setw(3);
|
os << std::setw(3);
|
||||||
if (i) os << i->size();
|
if (i) os << i->size();
|
||||||
else os << "-";
|
else os << "-";
|
||||||
}
|
}
|
||||||
// trajectories
|
// trajectories
|
||||||
os << std::setw(5) << direction<SubTask::READS_INPUT, SubTask::WRITES_PREV_OUTPUT>(stage)
|
os << std::setw(5) << direction<SubTaskPrivate::READS_START, SubTaskPrivate::WRITES_PREV_END>(stage)
|
||||||
<< std::setw(3) << stage.trajectories_.size()
|
<< std::setw(3) << stage.trajectories_.size()
|
||||||
<< std::setw(5) << direction<SubTask::READS_OUTPUT, SubTask::WRITES_NEXT_INPUT>(stage);
|
<< std::setw(5) << direction<SubTaskPrivate::READS_END, SubTaskPrivate::WRITES_NEXT_START>(stage);
|
||||||
// outputs
|
// ends
|
||||||
for (const Interface* i : {stage.output_.get(), stage.next_input_}) {
|
for (const Interface* i : {stage.ends_.get(), stage.next_starts_}) {
|
||||||
os << std::setw(3);
|
os << std::setw(3);
|
||||||
if (i) os << i->size();
|
if (i) os << i->size();
|
||||||
else os << "-";
|
else os << "-";
|
||||||
@ -67,164 +63,162 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
|||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr SubTask::scene() const
|
|
||||||
{
|
|
||||||
return pimpl_->scene_;
|
|
||||||
}
|
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr SubTask::planner() const
|
SubTaskPrivate::SubTaskPrivate(SubTask *me, const std::string &name)
|
||||||
{
|
: me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr)
|
||||||
return pimpl_->planner_;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SubTask::setPlanningScene(const planning_scene::PlanningSceneConstPtr &scene){
|
|
||||||
pimpl_->scene_= scene;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SubTask::setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr &planner){
|
|
||||||
pimpl_->planner_= planner;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
SubTaskPrivate::SubTaskPrivate(SubTask *me, const std::__cxx11::string &name)
|
|
||||||
: me_(me), name_(name), parent_(nullptr), prev_output_(nullptr), next_input_(nullptr)
|
|
||||||
{}
|
{}
|
||||||
|
|
||||||
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
|
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
|
||||||
trajectories_.emplace_back(trajectory);
|
trajectories_.emplace_back(trajectory);
|
||||||
return trajectories_.back();
|
SubTrajectory& back = trajectories_.back();
|
||||||
|
return back;
|
||||||
|
}
|
||||||
|
|
||||||
|
SubTaskPrivate::InterfaceFlags SubTaskPrivate::interfaceFlags() const
|
||||||
|
{
|
||||||
|
InterfaceFlags result = announcedFlags();
|
||||||
|
result &= ~InterfaceFlags(OWN_IF_MASK);
|
||||||
|
result |= deducedFlags();
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return the interface flags that can be deduced from the interface
|
// return the interface flags that can be deduced from the interface
|
||||||
inline SubTask::InterfaceFlags SubTaskPrivate::deducedFlags() const
|
inline SubTaskPrivate::InterfaceFlags SubTaskPrivate::deducedFlags() const
|
||||||
{
|
{
|
||||||
SubTask::InterfaceFlags f;
|
InterfaceFlags f;
|
||||||
if (input_) f |= SubTask::READS_INPUT;
|
if (starts_) f |= READS_START;
|
||||||
if (output_) f |= SubTask::READS_OUTPUT;
|
if (ends_) f |= READS_END;
|
||||||
if (prevOutput()) f |= SubTask::WRITES_PREV_OUTPUT;
|
if (prevEnds()) f |= WRITES_PREV_END;
|
||||||
if (nextInput()) f |= SubTask::WRITES_NEXT_INPUT;
|
if (nextStarts()) f |= WRITES_NEXT_START;
|
||||||
return f;
|
return f;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool SubTaskPrivate::sendForward(SubTrajectory &trajectory, const planning_scene::PlanningSceneConstPtr &ps){
|
|
||||||
std::cout << "sending state to start" << std::endl;
|
|
||||||
if (next_input_) {
|
|
||||||
next_input_->add(ps, &trajectory, NULL);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
inline bool SubTaskPrivate::sendBackward(SubTrajectory &trajectory, const planning_scene::PlanningSceneConstPtr &ps){
|
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
|
||||||
std::cout << "sending state to end" << std::endl;
|
|
||||||
if (prev_output_) {
|
|
||||||
prev_output_->add(ps, NULL, &trajectory);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
PropagatingAnyWayPrivate::PropagatingAnyWayPrivate(PropagatingAnyWay *me, PropagatingAnyWay::Direction dir, const std::__cxx11::string &name)
|
|
||||||
: SubTaskPrivate(me, name), dir(dir)
|
: SubTaskPrivate(me, name), dir(dir)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
SubTask::InterfaceFlags PropagatingAnyWayPrivate::announcedFlags() const {
|
SubTaskPrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
|
||||||
SubTask::InterfaceFlags f;
|
InterfaceFlags f;
|
||||||
if (dir & PropagatingAnyWay::FORWARD)
|
if (dir & PropagatingEitherWay::FORWARD)
|
||||||
f |= SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::WRITES_NEXT_INPUT});
|
f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
|
||||||
if (dir & PropagatingAnyWay::BACKWARD)
|
if (dir & PropagatingEitherWay::BACKWARD)
|
||||||
f |= SubTask::InterfaceFlags({SubTask::READS_OUTPUT, SubTask::WRITES_PREV_OUTPUT});
|
f |= InterfaceFlags({READS_END, WRITES_PREV_END});
|
||||||
return f;
|
return f;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool PropagatingAnyWayPrivate::hasStartState() const{
|
inline bool PropagatingEitherWayPrivate::hasStartState() const{
|
||||||
return next_input_state_ != input_->end();
|
return next_start_state_ != starts_->end();
|
||||||
}
|
}
|
||||||
|
|
||||||
const InterfaceState& PropagatingAnyWayPrivate::fetchStartState(){
|
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
|
||||||
if (!hasStartState())
|
if (!hasStartState())
|
||||||
throw std::runtime_error("no new state for beginning available");
|
throw std::runtime_error("no new state for beginning available");
|
||||||
|
|
||||||
const InterfaceState& state= *next_input_state_;
|
const InterfaceState& state= *next_start_state_;
|
||||||
++next_input_state_;
|
++next_start_state_;
|
||||||
|
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PropagatingAnyWayPrivate::sendForward(const robot_trajectory::RobotTrajectoryPtr& t,
|
inline bool PropagatingEitherWayPrivate::hasEndState() const{
|
||||||
const InterfaceState& from,
|
return next_end_state_ != ends_->end();
|
||||||
const planning_scene::PlanningSceneConstPtr& to,
|
|
||||||
double cost){
|
|
||||||
SubTrajectory &trajectory = addTrajectory(t, cost);
|
|
||||||
trajectory.setStartState(from);
|
|
||||||
return SubTaskPrivate::sendForward(trajectory, to);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
||||||
inline bool PropagatingAnyWayPrivate::hasEndState() const{
|
|
||||||
return next_output_state_ != output_->end();
|
|
||||||
}
|
|
||||||
|
|
||||||
const InterfaceState& PropagatingAnyWayPrivate::fetchEndState(){
|
|
||||||
if(!hasEndState())
|
if(!hasEndState())
|
||||||
throw std::runtime_error("no new state for ending available");
|
throw std::runtime_error("no new state for ending available");
|
||||||
|
|
||||||
const InterfaceState& state= *next_output_state_;
|
const InterfaceState& state= *next_end_state_;
|
||||||
++next_output_state_;
|
++next_end_state_;
|
||||||
|
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PropagatingAnyWayPrivate::sendBackward(const robot_trajectory::RobotTrajectoryPtr& t,
|
bool PropagatingEitherWayPrivate::canCompute() const
|
||||||
const planning_scene::PlanningSceneConstPtr& from,
|
{
|
||||||
const InterfaceState& to,
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
|
||||||
double cost){
|
return true;
|
||||||
SubTrajectory& trajectory = addTrajectory(t, cost);
|
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState())
|
||||||
trajectory.setEndState(to);
|
return true;
|
||||||
return SubTaskPrivate::sendBackward(trajectory, from);
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PropagatingEitherWayPrivate::compute()
|
||||||
|
{
|
||||||
|
PropagatingEitherWay* me = static_cast<PropagatingEitherWay*>(me_);
|
||||||
|
|
||||||
|
bool result = false;
|
||||||
|
planning_scene::PlanningScenePtr ps;
|
||||||
|
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
|
double cost;
|
||||||
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
|
||||||
|
if (me->computeForward(fetchStartState()))
|
||||||
|
result |= true;
|
||||||
|
}
|
||||||
|
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
|
||||||
|
if (me->computeBackward(fetchEndState()))
|
||||||
|
result |= true;
|
||||||
|
}
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PropagatingEitherWayPrivate::newStartState(const Interface::iterator &it)
|
||||||
|
{
|
||||||
|
// we just appended a state to the list, but the iterator doesn't see it anymore
|
||||||
|
// so let's point it at the new one
|
||||||
|
if(next_start_state_ == starts_->end())
|
||||||
|
--next_start_state_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PropagatingEitherWayPrivate::newEndState(const Interface::iterator &it)
|
||||||
|
{
|
||||||
|
// we just appended a state to the list, but the iterator doesn't see it anymore
|
||||||
|
// so let's point it at the new one
|
||||||
|
if(next_end_state_ == ends_->end())
|
||||||
|
--next_end_state_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
|
PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
|
||||||
: PropagatingAnyWay(new PropagatingAnyWayPrivate(this, ANYWAY, name))
|
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl)
|
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
|
||||||
: SubTask(impl)
|
: SubTask(impl)
|
||||||
{
|
{
|
||||||
initInterface();
|
initInterface();
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingAnyWay::initInterface()
|
void PropagatingEitherWay::initInterface()
|
||||||
{
|
{
|
||||||
IMPL(PropagatingAnyWay)
|
IMPL(PropagatingEitherWay)
|
||||||
if (impl->dir & PropagatingAnyWay::FORWARD) {
|
if (impl->dir & PropagatingEitherWay::FORWARD) {
|
||||||
if (!impl->input_) { // keep existing interface if possible
|
if (!impl->starts_) { // keep existing interface if possible
|
||||||
impl->input_.reset(new Interface([this](const Interface::iterator& it) { newInputState(it); }));
|
impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); }));
|
||||||
impl->next_input_state_ = impl->input_->begin();
|
impl->next_start_state_ = impl->starts_->begin();
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
impl->input_.reset();
|
impl->starts_.reset();
|
||||||
impl->next_input_state_ = Interface::iterator();
|
impl->next_start_state_ = Interface::iterator();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (impl->dir & PropagatingAnyWay::BACKWARD) {
|
if (impl->dir & PropagatingEitherWay::BACKWARD) {
|
||||||
if (!impl->output_) { // keep existing interface if possible
|
if (!impl->ends_) { // keep existing interface if possible
|
||||||
impl->output_.reset(new Interface([this](const Interface::iterator& it) { newOutputState(it); }));
|
impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); }));
|
||||||
impl->next_output_state_ = impl->output_->end();
|
impl->next_end_state_ = impl->ends_->end();
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
impl->output_.reset();
|
impl->ends_.reset();
|
||||||
impl->next_output_state_ = Interface::iterator();
|
impl->next_end_state_ = Interface::iterator();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingAnyWay::restrictDirection(PropagatingAnyWay::Direction dir)
|
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir)
|
||||||
{
|
{
|
||||||
IMPL(PropagatingAnyWay);
|
IMPL(PropagatingEitherWay);
|
||||||
if (impl->dir == dir) return;
|
if (impl->dir == dir) return;
|
||||||
if (impl->isConnected())
|
if (impl->isConnected())
|
||||||
throw std::runtime_error("Cannot change direction after being connected");
|
throw std::runtime_error("Cannot change direction after being connected");
|
||||||
@ -232,117 +226,81 @@ void PropagatingAnyWay::restrictDirection(PropagatingAnyWay::Direction dir)
|
|||||||
initInterface();
|
initInterface();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PropagatingAnyWay::computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to,
|
void PropagatingEitherWay::sendForward(const InterfaceState& from,
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost)
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
{
|
const robot_trajectory::RobotTrajectoryPtr& t,
|
||||||
// default implementation is void, needs override by user
|
double cost){
|
||||||
return false;
|
IMPL(PropagatingEitherWay)
|
||||||
|
std::cout << "sending state forward" << std::endl;
|
||||||
|
SubTrajectory &trajectory = impl->addTrajectory(t, cost);
|
||||||
|
trajectory.setStartState(from);
|
||||||
|
impl->nextStarts()->add(to, &trajectory, NULL);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PropagatingAnyWay::computeBackward(planning_scene::PlanningScenePtr &from, const InterfaceState &to,
|
void PropagatingEitherWay::sendBackward(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost)
|
const InterfaceState& to,
|
||||||
{
|
const robot_trajectory::RobotTrajectoryPtr& t,
|
||||||
// default implementation is void, needs override by user
|
double cost){
|
||||||
return false;
|
IMPL(PropagatingEitherWay)
|
||||||
}
|
std::cout << "sending state backward" << std::endl;
|
||||||
|
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
||||||
bool PropagatingAnyWay::canCompute() const
|
trajectory.setEndState(to);
|
||||||
{
|
impl->prevEnds()->add(from, NULL, &trajectory);
|
||||||
IMPL(const PropagatingAnyWay);
|
|
||||||
if ((impl->dir & PropagatingAnyWay::FORWARD) && impl->hasStartState())
|
|
||||||
return true;
|
|
||||||
if ((impl->dir & PropagatingAnyWay::BACKWARD) && impl->hasEndState())
|
|
||||||
return true;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool PropagatingAnyWay::compute()
|
|
||||||
{
|
|
||||||
IMPL(PropagatingAnyWay);
|
|
||||||
bool result = false;
|
|
||||||
planning_scene::PlanningScenePtr ps;
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
|
||||||
double cost;
|
|
||||||
if ((impl->dir & PropagatingAnyWay::FORWARD) && impl->hasStartState()) {
|
|
||||||
const InterfaceState& from = impl->fetchStartState();
|
|
||||||
if (computeForward(from, ps, trajectory, cost) && ps
|
|
||||||
&& impl->sendForward(trajectory, from, ps, cost))
|
|
||||||
result |= true;
|
|
||||||
}
|
|
||||||
if ((impl->dir & PropagatingAnyWay::BACKWARD) && impl->hasEndState()) {
|
|
||||||
const InterfaceState& to = impl->fetchEndState();
|
|
||||||
if (computeBackward(ps, to, trajectory, cost) && ps
|
|
||||||
&& impl->sendBackward(trajectory, ps, to, cost))
|
|
||||||
result |= true;
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
void PropagatingAnyWay::newInputState(const Interface::iterator &it)
|
|
||||||
{
|
|
||||||
IMPL(PropagatingAnyWay);
|
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
|
||||||
// so let's point it at the new one
|
|
||||||
if(impl->next_input_state_ == impl->input_->end())
|
|
||||||
--impl->next_input_state_;
|
|
||||||
}
|
|
||||||
|
|
||||||
void PropagatingAnyWay::newOutputState(const Interface::iterator &it)
|
|
||||||
{
|
|
||||||
IMPL(PropagatingAnyWay);
|
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
|
||||||
// so let's point it at the new one
|
|
||||||
if( impl->next_output_state_ == impl->output_->end() )
|
|
||||||
--impl->next_output_state_;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, const std::__cxx11::string &name)
|
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, const std::string &name)
|
||||||
: PropagatingAnyWayPrivate(me, PropagatingAnyWay::FORWARD, name)
|
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::FORWARD, name)
|
||||||
{
|
{
|
||||||
// indicate, that we don't accept new states from output interface
|
// indicate, that we don't accept new states from ends_ interface
|
||||||
output_.reset();
|
ends_.reset();
|
||||||
next_output_state_ = Interface::iterator();
|
next_end_state_ = Interface::iterator();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
PropagatingForward::PropagatingForward(const std::string& name)
|
PropagatingForward::PropagatingForward(const std::string& name)
|
||||||
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name))
|
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
bool PropagatingForward::computeBackward(const InterfaceState &to)
|
||||||
PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, const std::__cxx11::string &name)
|
|
||||||
: PropagatingAnyWayPrivate(me, PropagatingAnyWay::BACKWARD, name)
|
|
||||||
{
|
{
|
||||||
// indicate, that we don't accept new states from input interface
|
assert(false); // This should never be called
|
||||||
input_.reset();
|
}
|
||||||
next_input_state_ = Interface::iterator();
|
|
||||||
|
|
||||||
|
PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name)
|
||||||
|
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::BACKWARD, name)
|
||||||
|
{
|
||||||
|
// indicate, that we don't accept new states from starts_ interface
|
||||||
|
starts_.reset();
|
||||||
|
next_start_state_ = Interface::iterator();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
PropagatingBackward::PropagatingBackward(const std::string &name)
|
PropagatingBackward::PropagatingBackward(const std::string &name)
|
||||||
: PropagatingAnyWay(new PropagatingBackwardPrivate(this, name))
|
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
bool PropagatingBackward::computeForward(const InterfaceState &from)
|
||||||
|
{
|
||||||
|
assert(false); // This should never be called
|
||||||
|
}
|
||||||
|
|
||||||
GeneratorPrivate::GeneratorPrivate(Generator *me, const std::__cxx11::string &name)
|
|
||||||
|
GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
|
||||||
: SubTaskPrivate(me, name)
|
: SubTaskPrivate(me, name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
SubTask::InterfaceFlags GeneratorPrivate::announcedFlags() const {
|
SubTaskPrivate::InterfaceFlags GeneratorPrivate::announcedFlags() const {
|
||||||
return SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT,SubTask::WRITES_PREV_OUTPUT});
|
return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END});
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool GeneratorPrivate::spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost)
|
bool GeneratorPrivate::canCompute() const {
|
||||||
{
|
return static_cast<Generator*>(me_)->canCompute();
|
||||||
// empty trajectory ref -> this node only produces states
|
}
|
||||||
robot_trajectory::RobotTrajectoryPtr dummy;
|
|
||||||
SubTrajectory& trajectory = addTrajectory(dummy, cost);
|
|
||||||
|
|
||||||
std::cout << "spawning state" << std::endl;
|
bool GeneratorPrivate::compute() {
|
||||||
bool result = sendBackward(trajectory, ps);
|
return static_cast<Generator*>(me_)->compute();
|
||||||
result |= sendForward(trajectory, ps);
|
|
||||||
return result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -350,30 +308,55 @@ Generator::Generator(const std::string &name)
|
|||||||
: SubTask(new GeneratorPrivate(this, name))
|
: SubTask(new GeneratorPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
|
||||||
bool Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
|
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
|
||||||
{
|
{
|
||||||
|
std::cout << "spawning state forwards and backwards" << std::endl;
|
||||||
IMPL(Generator)
|
IMPL(Generator)
|
||||||
return impl->spawn(ps, cost);
|
// empty trajectory ref -> this node only produces states
|
||||||
|
robot_trajectory::RobotTrajectoryPtr dummy;
|
||||||
|
SubTrajectory& trajectory = impl->addTrajectory(dummy, cost);
|
||||||
|
impl->prevEnds()->add(ps, NULL, &trajectory);
|
||||||
|
impl->nextStarts()->add(ps, &trajectory, NULL);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::__cxx11::string &name)
|
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
|
||||||
: SubTaskPrivate(me, name)
|
: SubTaskPrivate(me, name)
|
||||||
{
|
{
|
||||||
input_.reset(new Interface(std::bind(&Connecting::newInputState, me, std::placeholders::_1)));
|
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
|
||||||
output_.reset(new Interface(std::bind(&Connecting::newOutputState, me, std::placeholders::_1)));
|
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
|
||||||
it_pairs_ = std::make_pair(input_->begin(), output_->begin());
|
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
|
||||||
}
|
}
|
||||||
|
|
||||||
SubTask::InterfaceFlags ConnectingPrivate::announcedFlags() const {
|
SubTaskPrivate::InterfaceFlags ConnectingPrivate::announcedFlags() const {
|
||||||
return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::READS_OUTPUT});
|
return InterfaceFlags({READS_START, READS_END});
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void ConnectingPrivate::connect(const robot_trajectory::RobotTrajectoryPtr &t, const InterfaceStatePair &state_pair, double cost)
|
void ConnectingPrivate::newStartState(const Interface::iterator& it)
|
||||||
{
|
{
|
||||||
SubTrajectory& trajectory = addTrajectory(t, cost);
|
// TODO: need to handle the pairs iterator
|
||||||
trajectory.setStartState(state_pair.first);
|
if(it_pairs_.first == starts_->end())
|
||||||
trajectory.setEndState(state_pair.second);
|
--it_pairs_.first;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ConnectingPrivate::newEndState(const Interface::iterator& it)
|
||||||
|
{
|
||||||
|
// TODO: need to handle the pairs iterator properly
|
||||||
|
if(it_pairs_.second == ends_->end())
|
||||||
|
--it_pairs_.second;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ConnectingPrivate::canCompute() const{
|
||||||
|
// TODO: implement this properly
|
||||||
|
return it_pairs_.first != starts_->end() &&
|
||||||
|
it_pairs_.second != ends_->end();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ConnectingPrivate::compute() {
|
||||||
|
// TODO: implement this properly
|
||||||
|
const InterfaceState& from = *it_pairs_.first;
|
||||||
|
const InterfaceState& to = *(it_pairs_.second++);
|
||||||
|
return static_cast<Connecting*>(me_)->compute(from, to);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -382,40 +365,12 @@ Connecting::Connecting(const std::string &name)
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void Connecting::newInputState(const Interface::iterator& it)
|
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
|
||||||
{
|
const robot_trajectory::RobotTrajectoryPtr& t, double cost) {
|
||||||
IMPL(Connecting);
|
|
||||||
// TODO: need to handle the pairs iterator
|
|
||||||
if( impl->it_pairs_.first == impl->input_->end() )
|
|
||||||
--impl->it_pairs_.first;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Connecting::newOutputState(const Interface::iterator& it)
|
|
||||||
{
|
|
||||||
IMPL(Connecting);
|
|
||||||
// TODO: need to handle the pairs iterator properly
|
|
||||||
if( impl->it_pairs_.second == impl->output_->end() )
|
|
||||||
--impl->it_pairs_.second;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Connecting::hasStatePair() const{
|
|
||||||
IMPL(const Connecting);
|
|
||||||
// TODO: implement this properly
|
|
||||||
return impl->it_pairs_.first != impl->input_->end() &&
|
|
||||||
impl->it_pairs_.second != impl->output_->end();
|
|
||||||
}
|
|
||||||
|
|
||||||
InterfaceStatePair Connecting::fetchStatePair(){
|
|
||||||
IMPL(Connecting);
|
|
||||||
// TODO: implement this properly
|
|
||||||
return std::pair<const InterfaceState&, const InterfaceState&>
|
|
||||||
(*impl->it_pairs_.first, *(impl->it_pairs_.second++));
|
|
||||||
}
|
|
||||||
|
|
||||||
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
|
|
||||||
{
|
|
||||||
IMPL(Connecting)
|
IMPL(Connecting)
|
||||||
impl->connect(t, state_pair, cost);
|
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
||||||
|
trajectory.setStartState(from);
|
||||||
|
trajectory.setEndState(to);
|
||||||
}
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
110
src/subtask_p.h
110
src/subtask_p.h
@ -15,104 +15,106 @@ class SubTaskPrivate {
|
|||||||
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef std::list<SubTask::pointer> array_type;
|
typedef std::list<SubTask::pointer> container_type;
|
||||||
|
|
||||||
SubTaskPrivate(SubTask* me, const std::string& name);
|
SubTaskPrivate(SubTask* me, const std::string& name);
|
||||||
|
|
||||||
SubTask::InterfaceFlags deducedFlags() const;
|
enum InterfaceFlag {
|
||||||
virtual SubTask::InterfaceFlags announcedFlags() const = 0;
|
READS_START = 0x01,
|
||||||
|
READS_END = 0x02,
|
||||||
|
WRITES_NEXT_START = 0x04,
|
||||||
|
WRITES_PREV_END = 0x08,
|
||||||
|
|
||||||
|
OWN_IF_MASK = READS_START | READS_END,
|
||||||
|
EXT_IF_MASK = WRITES_NEXT_START | WRITES_PREV_END,
|
||||||
|
INPUT_IF_MASK = READS_START | WRITES_PREV_END,
|
||||||
|
OUTPUT_IF_MASK = READS_END | WRITES_NEXT_START,
|
||||||
|
};
|
||||||
|
typedef Flags<InterfaceFlag> InterfaceFlags;
|
||||||
|
|
||||||
|
InterfaceFlags interfaceFlags() const;
|
||||||
|
InterfaceFlags deducedFlags() const;
|
||||||
|
virtual InterfaceFlags announcedFlags() const = 0;
|
||||||
std::list<SubTrajectory>& trajectories() { return trajectories_; }
|
std::list<SubTrajectory>& trajectories() { return trajectories_; }
|
||||||
|
|
||||||
|
virtual bool canCompute() const = 0;
|
||||||
|
virtual bool compute() = 0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
SubTask* const me_; // associated/owning SubTask instance
|
SubTask* const me_; // associated/owning SubTask instance
|
||||||
const std::string name_;
|
const std::string name_;
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
InterfacePtr starts_;
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
InterfacePtr ends_;
|
||||||
|
|
||||||
InterfacePtr input_;
|
inline ContainerBasePrivate* parent() const { return parent_; }
|
||||||
InterfacePtr output_;
|
inline bool isConnected() const { return prev_ends_ || next_starts_; }
|
||||||
|
inline Interface* prevEnds() const { return prev_ends_; }
|
||||||
inline const Interface* prevOutput() const { return prev_output_; }
|
inline Interface* nextStarts() const { return next_starts_; }
|
||||||
inline const Interface* nextInput() const { return next_input_; }
|
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
||||||
inline bool isConnected() const { return prev_output_ || next_input_; }
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::list<SubTrajectory> trajectories_;
|
std::list<SubTrajectory> trajectories_;
|
||||||
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
|
||||||
bool sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
|
||||||
bool sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// !! items accessed only by ContainerBasePrivate to maintain hierarchy !!
|
// !! items accessed only by ContainerBasePrivate to maintain hierarchy !!
|
||||||
ContainerBasePrivate* parent_; // owning parent
|
ContainerBasePrivate* parent_; // owning parent
|
||||||
array_type::iterator it_; // iterator into parent's children_ list referring to this
|
container_type::iterator it_; // iterator into parent's children_ list referring to this
|
||||||
// caching the pointers to the output_ / input_ interface of previous / next stage
|
// caching the pointers to the ends_ / starts_ interface of previous / next stage
|
||||||
mutable Interface *prev_output_; // interface to be used for sendBackward()
|
mutable Interface *prev_ends_; // interface to be used for sendBackward()
|
||||||
mutable Interface *next_input_; // interface to be use for sendForward()
|
mutable Interface *next_starts_; // interface to be use for sendForward()
|
||||||
};
|
};
|
||||||
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||||
|
|
||||||
|
|
||||||
class PropagatingAnyWayPrivate : public SubTaskPrivate {
|
class PropagatingEitherWayPrivate : public SubTaskPrivate {
|
||||||
friend class PropagatingAnyWay;
|
friend class PropagatingEitherWay;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PropagatingAnyWay::Direction dir;
|
PropagatingEitherWay::Direction dir;
|
||||||
|
|
||||||
inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, PropagatingAnyWay::Direction dir,
|
inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir,
|
||||||
const std::string &name);
|
const std::string &name);
|
||||||
SubTask::InterfaceFlags announcedFlags() const override;
|
InterfaceFlags announcedFlags() const override;
|
||||||
|
|
||||||
|
bool canCompute() const override;
|
||||||
|
bool compute() override;
|
||||||
|
|
||||||
bool hasStartState() const;
|
bool hasStartState() const;
|
||||||
const InterfaceState &fetchStartState();
|
const InterfaceState &fetchStartState();
|
||||||
bool sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
|
||||||
const InterfaceState& from,
|
|
||||||
const planning_scene::PlanningSceneConstPtr& to,
|
|
||||||
double cost = 0);
|
|
||||||
|
|
||||||
bool hasEndState() const;
|
bool hasEndState() const;
|
||||||
const InterfaceState &fetchEndState();
|
const InterfaceState &fetchEndState();
|
||||||
bool sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
|
||||||
const planning_scene::PlanningSceneConstPtr& from,
|
|
||||||
const InterfaceState& to,
|
|
||||||
double cost = 0);
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
Interface::const_iterator next_input_state_;
|
// get informed when new start or end state becomes available
|
||||||
Interface::const_iterator next_output_state_;
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
|
void newEndState(const std::list<InterfaceState>::iterator& it);
|
||||||
|
|
||||||
|
Interface::const_iterator next_start_state_;
|
||||||
|
Interface::const_iterator next_end_state_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
class PropagatingForwardPrivate : public PropagatingAnyWayPrivate {
|
class PropagatingForwardPrivate : public PropagatingEitherWayPrivate {
|
||||||
public:
|
public:
|
||||||
inline PropagatingForwardPrivate(PropagatingForward *me, const std::string &name);
|
inline PropagatingForwardPrivate(PropagatingForward *me, const std::string &name);
|
||||||
|
|
||||||
private:
|
|
||||||
// restrict access to backward methods to provide compile-time check
|
|
||||||
using PropagatingAnyWayPrivate::hasEndState;
|
|
||||||
using PropagatingAnyWayPrivate::fetchEndState;
|
|
||||||
using PropagatingAnyWayPrivate::sendBackward;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
class PropagatingBackwardPrivate : public PropagatingAnyWayPrivate {
|
class PropagatingBackwardPrivate : public PropagatingEitherWayPrivate {
|
||||||
public:
|
public:
|
||||||
inline PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name);
|
inline PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name);
|
||||||
|
|
||||||
private:
|
|
||||||
// restrict access to forward method to provide compile-time check
|
|
||||||
using PropagatingAnyWayPrivate::hasStartState;
|
|
||||||
using PropagatingAnyWayPrivate::fetchStartState;
|
|
||||||
using PropagatingAnyWayPrivate::sendForward;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
class GeneratorPrivate : public SubTaskPrivate {
|
class GeneratorPrivate : public SubTaskPrivate {
|
||||||
public:
|
public:
|
||||||
inline GeneratorPrivate(Generator *me, const std::string &name);
|
inline GeneratorPrivate(Generator *me, const std::string &name);
|
||||||
SubTask::InterfaceFlags announcedFlags() const override;
|
InterfaceFlags announcedFlags() const override;
|
||||||
bool spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost);
|
|
||||||
|
bool canCompute() const override;
|
||||||
|
bool compute() override;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -121,11 +123,19 @@ class ConnectingPrivate : public SubTaskPrivate {
|
|||||||
|
|
||||||
public:
|
public:
|
||||||
inline ConnectingPrivate(Connecting *me, const std::string &name);
|
inline ConnectingPrivate(Connecting *me, const std::string &name);
|
||||||
SubTask::InterfaceFlags announcedFlags() const override;
|
InterfaceFlags announcedFlags() const override;
|
||||||
|
|
||||||
|
bool canCompute() const override;
|
||||||
|
bool compute() override;
|
||||||
|
|
||||||
void connect(const robot_trajectory::RobotTrajectoryPtr& t,
|
void connect(const robot_trajectory::RobotTrajectoryPtr& t,
|
||||||
const InterfaceStatePair& state_pair, double cost);
|
const InterfaceStatePair& state_pair, double cost);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
// get informed when new start or end state becomes available
|
||||||
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
|
void newEndState(const std::list<InterfaceState>::iterator& it);
|
||||||
|
|
||||||
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -16,7 +16,7 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
||||||
: PropagatingAnyWay(name),
|
: PropagatingEitherWay(name),
|
||||||
step_size_(0.005)
|
step_size_(0.005)
|
||||||
{
|
{
|
||||||
ros::NodeHandle nh;
|
ros::NodeHandle nh;
|
||||||
@ -70,9 +70,8 @@ namespace {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CartesianPositionMotion::computeForward(const InterfaceState &beginning, planning_scene::PlanningScenePtr &result_scene,
|
bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||||
robot_trajectory::RobotTrajectoryPtr &traj, double &cost){
|
planning_scene::PlanningScenePtr result_scene = from.state->diff();
|
||||||
result_scene = beginning.state->diff();
|
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
||||||
@ -90,7 +89,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &beginning, pl
|
|||||||
bool succeeded= false;
|
bool succeeded= false;
|
||||||
|
|
||||||
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
|
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
|
||||||
const Eigen::Affine3d& frame= beginning.state->getFrameTransform(towards_.header.frame_id);
|
const Eigen::Affine3d& frame= from.state->getFrameTransform(towards_.header.frame_id);
|
||||||
|
|
||||||
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
|
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
|
||||||
|
|
||||||
@ -144,21 +143,22 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &beginning, pl
|
|||||||
|
|
||||||
|
|
||||||
if(succeeded){
|
if(succeeded){
|
||||||
traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
robot_trajectory::RobotTrajectoryPtr traj
|
||||||
|
= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||||
for( auto& tp : trajectory_steps )
|
for( auto& tp : trajectory_steps )
|
||||||
traj->addSuffixWayPoint(tp, 0.0);
|
traj->addSuffixWayPoint(tp, 0.0);
|
||||||
|
sendForward(from, result_scene, traj);
|
||||||
|
|
||||||
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
||||||
robot_state= *result_state;
|
robot_state= *result_state;
|
||||||
_publishTrajectory(*traj, *result_state);
|
_publishTrajectory(result_scene, *traj, *result_state);
|
||||||
}
|
}
|
||||||
|
|
||||||
return succeeded;
|
return succeeded;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CartesianPositionMotion::computeBackward(planning_scene::PlanningScenePtr &result_scene, const InterfaceState &ending,
|
bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
||||||
robot_trajectory::RobotTrajectoryPtr &traj, double &cost){
|
planning_scene::PlanningScenePtr result_scene = to.state->diff();
|
||||||
result_scene = ending.state->diff();
|
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
||||||
@ -210,24 +210,27 @@ bool CartesianPositionMotion::computeBackward(planning_scene::PlanningScenePtr &
|
|||||||
bool succeeded= achieved_distance >= min_distance_;
|
bool succeeded= achieved_distance >= min_distance_;
|
||||||
|
|
||||||
if(succeeded){
|
if(succeeded){
|
||||||
traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||||
for( auto& tp : trajectory_steps )
|
for( auto& tp : trajectory_steps )
|
||||||
traj->addPrefixWayPoint(tp, 0.0);
|
traj->addPrefixWayPoint(tp, 0.0);
|
||||||
|
sendBackward(result_scene, to, traj);
|
||||||
|
|
||||||
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
||||||
robot_state= *result_state;
|
robot_state= *result_state;
|
||||||
|
|
||||||
_publishTrajectory(*traj, *result_state);
|
_publishTrajectory(result_scene, *traj, *result_state);
|
||||||
}
|
}
|
||||||
|
|
||||||
return succeeded;
|
return succeeded;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
|
void CartesianPositionMotion::_publishTrajectory(const planning_scene::PlanningSceneConstPtr& scene,
|
||||||
|
const robot_trajectory::RobotTrajectory& trajectory,
|
||||||
|
const moveit::core::RobotState& start){
|
||||||
moveit_msgs::DisplayTrajectory dt;
|
moveit_msgs::DisplayTrajectory dt;
|
||||||
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
|
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
|
||||||
dt.model_id= scene()->getRobotModel()->getName();
|
dt.model_id= scene->getRobotModel()->getName();
|
||||||
dt.trajectory.resize(1);
|
dt.trajectory.resize(1);
|
||||||
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
|
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
|
||||||
dt.model_id= start.getRobotModel()->getName();
|
dt.model_id= start.getRobotModel()->getName();
|
||||||
|
|||||||
@ -8,13 +8,19 @@ CurrentState::CurrentState(std::string name)
|
|||||||
ran_= false;
|
ran_= false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
scene_ = scene;
|
||||||
|
ran_= false;
|
||||||
|
}
|
||||||
|
|
||||||
bool CurrentState::canCompute() const{
|
bool CurrentState::canCompute() const{
|
||||||
return !ran_;
|
return !ran_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CurrentState::compute(){
|
bool CurrentState::compute(){
|
||||||
ran_= true;
|
ran_= true;
|
||||||
spawn(scene());
|
spawn(scene_);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -33,6 +33,11 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
|
|||||||
ros::Duration(1.0).sleep();
|
ros::Duration(1.0).sleep();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
scene_ = scene;
|
||||||
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setGroup(std::string group){
|
void GenerateGraspPose::setGroup(std::string group){
|
||||||
group_= group;
|
group_= group;
|
||||||
}
|
}
|
||||||
@ -102,9 +107,9 @@ namespace {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool GenerateGraspPose::compute(){
|
bool GenerateGraspPose::compute(){
|
||||||
assert(scene()->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf");
|
assert(scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf");
|
||||||
|
|
||||||
planning_scene::PlanningScenePtr grasp_scene = scene()->diff();
|
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
|
||||||
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
|
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
|
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
|
||||||
@ -122,7 +127,7 @@ bool GenerateGraspPose::compute(){
|
|||||||
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
||||||
std::bind(
|
std::bind(
|
||||||
&isValid,
|
&isValid,
|
||||||
scene(),
|
scene_,
|
||||||
ignore_collisions_,
|
ignore_collisions_,
|
||||||
&previous_solutions_,
|
&previous_solutions_,
|
||||||
std::placeholders::_1,
|
std::placeholders::_1,
|
||||||
@ -130,7 +135,7 @@ bool GenerateGraspPose::compute(){
|
|||||||
std::placeholders::_3);
|
std::placeholders::_3);
|
||||||
|
|
||||||
geometry_msgs::Pose object_pose, grasp_pose;
|
geometry_msgs::Pose object_pose, grasp_pose;
|
||||||
const Eigen::Affine3d object_pose_eigen= scene()->getFrameTransform(object_);
|
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_);
|
||||||
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
||||||
throw std::runtime_error("requested object does not exist or could not be retrieved");
|
throw std::runtime_error("requested object does not exist or could not be retrieved");
|
||||||
|
|
||||||
|
|||||||
@ -1,5 +1,6 @@
|
|||||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||||
#include <moveit_task_constructor/storage.h>
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
#include <moveit_task_constructor/task.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
@ -13,9 +14,14 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
Gripper::Gripper(std::string name)
|
Gripper::Gripper(std::string name)
|
||||||
: PropagatingAnyWay(name)
|
: PropagatingEitherWay(name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
bool Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
planner_ = Task::createPlanner(scene->getRobotModel());
|
||||||
|
}
|
||||||
|
|
||||||
void Gripper::setEndEffector(std::string eef){
|
void Gripper::setEndEffector(std::string eef){
|
||||||
eef_= eef;
|
eef_= eef;
|
||||||
}
|
}
|
||||||
@ -64,7 +70,7 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
|
|||||||
}
|
}
|
||||||
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
if(!planner()->generatePlan(scene, req, res))
|
if(!planner_->generatePlan(scene, req, res))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
trajectory = res.trajectory_;
|
trajectory = res.trajectory_;
|
||||||
@ -83,14 +89,27 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Gripper::computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to,
|
bool Gripper::computeForward(const InterfaceState &from){
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost){
|
planning_scene::PlanningScenePtr to;
|
||||||
return compute(from, to, trajectory, cost, FORWARD);
|
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
|
double cost = 0;
|
||||||
|
|
||||||
|
if (!compute(from, to, trajectory, cost, FORWARD))
|
||||||
|
return false;
|
||||||
|
sendForward(from, to, trajectory, cost);
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
bool Gripper::computeBackward(planning_scene::PlanningScenePtr &from, const InterfaceState &to,
|
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost)
|
bool Gripper::computeBackward(const InterfaceState &to)
|
||||||
{
|
{
|
||||||
return compute(to, from, trajectory, cost, BACKWARD);
|
planning_scene::PlanningScenePtr from;
|
||||||
|
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
|
double cost = 0;
|
||||||
|
|
||||||
|
if (!compute(to, from, trajectory, cost, BACKWARD))
|
||||||
|
return false;
|
||||||
|
sendBackward(from, to, trajectory, cost);
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -1,5 +1,6 @@
|
|||||||
#include <moveit_task_constructor/subtasks/move.h>
|
#include <moveit_task_constructor/subtasks/move.h>
|
||||||
#include <moveit_task_constructor/storage.h>
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
#include <moveit_task_constructor/task.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
@ -17,15 +18,16 @@ Move::Move(std::string name)
|
|||||||
timeout_(5.0)
|
timeout_(5.0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
bool Move::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
planner_ = Task::createPlanner(scene->getRobotModel());
|
||||||
|
}
|
||||||
|
|
||||||
void Move::setGroup(std::string group){
|
void Move::setGroup(std::string group){
|
||||||
group_= group;
|
group_= group;
|
||||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
|
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Move::setLink(std::string link){
|
|
||||||
link_= link;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Move::setPlannerId(std::string planner){
|
void Move::setPlannerId(std::string planner){
|
||||||
planner_id_= planner;
|
planner_id_= planner;
|
||||||
}
|
}
|
||||||
@ -34,24 +36,8 @@ void Move::setTimeout(double timeout){
|
|||||||
timeout_= timeout;
|
timeout_= timeout;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* TODO: implement this in compute
|
bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||||
void Move::setFrom(std::string named_target){
|
mgi_->setJointValueTarget(to.state->getCurrentState());
|
||||||
from_named_target_= named_target;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Move::setTo(std::string named_target){
|
|
||||||
to_named_target_= named_target;
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
|
|
||||||
bool Move::canCompute() const{
|
|
||||||
return hasStatePair();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Move::compute(){
|
|
||||||
InterfaceStatePair state_pair= fetchStatePair();
|
|
||||||
|
|
||||||
mgi_->setJointValueTarget(state_pair.second.state->getCurrentState());
|
|
||||||
if( !planner_id_.empty() )
|
if( !planner_id_.empty() )
|
||||||
mgi_->setPlannerId(planner_id_);
|
mgi_->setPlannerId(planner_id_);
|
||||||
mgi_->setPlanningTime(timeout_);
|
mgi_->setPlanningTime(timeout_);
|
||||||
@ -61,11 +47,11 @@ bool Move::compute(){
|
|||||||
|
|
||||||
ros::Duration(4.0).sleep();
|
ros::Duration(4.0).sleep();
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
if(!planner()->generatePlan(state_pair.first.state, req, res))
|
if(!planner_->generatePlan(from.state, req, res))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
// finish subtask
|
// finish subtask
|
||||||
connect(res.trajectory_, state_pair);
|
connect(from, to, res.trajectory_);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
37
src/task.cpp
37
src/task.cpp
@ -16,14 +16,13 @@ namespace moveit { namespace task_constructor {
|
|||||||
class TaskPrivate : public SerialContainerPrivate {
|
class TaskPrivate : public SerialContainerPrivate {
|
||||||
friend class Task;
|
friend class Task;
|
||||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||||
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
TaskPrivate(Task* me, const std::string &name)
|
TaskPrivate(Task* me, const std::string &name)
|
||||||
: SerialContainerPrivate(me, name)
|
: SerialContainerPrivate(me, name)
|
||||||
{
|
{
|
||||||
initModel();
|
initModel();
|
||||||
initScene();
|
|
||||||
initPlanner();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void initModel () {
|
void initModel () {
|
||||||
@ -60,11 +59,6 @@ public:
|
|||||||
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
|
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
|
||||||
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
|
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
|
||||||
}
|
}
|
||||||
void initPlanner() {
|
|
||||||
assert(rml_);
|
|
||||||
assert(scene_);
|
|
||||||
planner_ = std::make_shared<planning_pipeline::PlanningPipeline>(rml_->getModel(), ros::NodeHandle("move_group"));
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -73,6 +67,27 @@ Task::Task(const std::string &name)
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
planning_pipeline::PlanningPipelinePtr
|
||||||
|
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
||||||
|
const std::string& planning_plugin_param_name,
|
||||||
|
const std::string& adapter_plugins_param_name) {
|
||||||
|
typedef std::tuple<std::string, std::string, std::string, std::string> PlannerID;
|
||||||
|
static std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> > planner_cache;
|
||||||
|
|
||||||
|
PlannerID id (model->getName(), ns, planning_plugin_param_name, adapter_plugins_param_name);
|
||||||
|
auto it = planner_cache.find(id);
|
||||||
|
|
||||||
|
planning_pipeline::PlanningPipelinePtr planner;
|
||||||
|
if (it != planner_cache.cend())
|
||||||
|
planner = it->second.lock();
|
||||||
|
if (!planner) {
|
||||||
|
planner = std::make_shared<planning_pipeline::PlanningPipeline>
|
||||||
|
(model, ros::NodeHandle(ns), planning_plugin_param_name, adapter_plugins_param_name);
|
||||||
|
planner_cache[id] = planner;
|
||||||
|
}
|
||||||
|
return planner;
|
||||||
|
}
|
||||||
|
|
||||||
void Task::add(pointer &&stage) {
|
void Task::add(pointer &&stage) {
|
||||||
if (!stage)
|
if (!stage)
|
||||||
throw std::runtime_error("stage insertion failed: invalid stage pointer");
|
throw std::runtime_error("stage insertion failed: invalid stage pointer");
|
||||||
@ -81,8 +96,11 @@ void Task::add(pointer &&stage) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Task::plan(){
|
bool Task::plan(){
|
||||||
|
IMPL(Task);
|
||||||
NewSolutionPublisher debug(*this);
|
NewSolutionPublisher debug(*this);
|
||||||
|
|
||||||
|
impl->initScene();
|
||||||
|
this->init(impl->scene_);
|
||||||
while(ros::ok() && canCompute()) {
|
while(ros::ok() && canCompute()) {
|
||||||
if (compute()) {
|
if (compute()) {
|
||||||
debug.publish();
|
debug.publish();
|
||||||
@ -94,7 +112,8 @@ bool Task::plan(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
const robot_state::RobotState& Task::getCurrentRobotState() const {
|
const robot_state::RobotState& Task::getCurrentRobotState() const {
|
||||||
return pimpl_func()->scene_->getCurrentState();
|
IMPL(const Task)
|
||||||
|
return impl->scene_->getCurrentState();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::printState(){
|
void Task::printState(){
|
||||||
@ -135,7 +154,7 @@ bool traverseFullTrajectories(
|
|||||||
|
|
||||||
bool Task::processSolutions(const Task::SolutionCallback& processor) {
|
bool Task::processSolutions(const Task::SolutionCallback& processor) {
|
||||||
IMPL(Task);
|
IMPL(Task);
|
||||||
const TaskPrivate::array_type& children = impl->children();
|
const TaskPrivate::container_type& children = impl->children();
|
||||||
const size_t nr_of_trajectories = children.size();
|
const size_t nr_of_trajectories = children.size();
|
||||||
std::vector<SubTrajectory*> trace;
|
std::vector<SubTrajectory*> trace;
|
||||||
trace.reserve(nr_of_trajectories);
|
trace.reserve(nr_of_trajectories);
|
||||||
|
|||||||
@ -29,8 +29,7 @@ public:
|
|||||||
class TestPropagatingForward : public PropagatingForward {
|
class TestPropagatingForward : public PropagatingForward {
|
||||||
public:
|
public:
|
||||||
TestPropagatingForward() : PropagatingForward("forwarder") {}
|
TestPropagatingForward() : PropagatingForward("forwarder") {}
|
||||||
bool canCompute() const override { return true; }
|
bool computeForward(const InterfaceState &from) override { return false; }
|
||||||
bool compute() override { return false; }
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@ -79,7 +78,9 @@ protected:
|
|||||||
|
|
||||||
TEST_F(BaseTest, interfaceFlags) {
|
TEST_F(BaseTest, interfaceFlags) {
|
||||||
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
|
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
|
||||||
EXPECT_EQ(g->interfaceFlags(), SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT, SubTask::WRITES_PREV_OUTPUT}));
|
EXPECT_EQ(g->pimpl_func()->interfaceFlags(),
|
||||||
|
SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START,
|
||||||
|
SubTaskPrivate::WRITES_PREV_END}));
|
||||||
}
|
}
|
||||||
|
|
||||||
#define VALIDATE(...) \
|
#define VALIDATE(...) \
|
||||||
@ -90,8 +91,8 @@ TEST_F(BaseTest, serialContainer) {
|
|||||||
SerialContainer c("serial");
|
SerialContainer c("serial");
|
||||||
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(c.pimpl_func());
|
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(c.pimpl_func());
|
||||||
|
|
||||||
EXPECT_TRUE(bool(cp->input_));
|
EXPECT_TRUE(bool(cp->starts_));
|
||||||
EXPECT_TRUE(bool(cp->output_));
|
EXPECT_TRUE(bool(cp->ends_));
|
||||||
EXPECT_EQ(parent(cp), nullptr);
|
EXPECT_EQ(parent(cp), nullptr);
|
||||||
EXPECT_EQ(parent(cp), nullptr);
|
EXPECT_EQ(parent(cp), nullptr);
|
||||||
VALIDATE();
|
VALIDATE();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user