mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
renamed subtask -> stage
This commit is contained in:
parent
3cc112fe2c
commit
a32007613e
@ -24,21 +24,21 @@ set(CMAKE_CXX_STANDARD 14)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/storage.cpp
|
||||
src/subtask.cpp
|
||||
src/stage.cpp
|
||||
src/container.cpp
|
||||
src/task.cpp
|
||||
src/debug.cpp
|
||||
|
||||
src/subtask_p.h
|
||||
src/stage_p.h
|
||||
src/container_p.h
|
||||
include/${PROJECT_NAME}/utils.h
|
||||
include/${PROJECT_NAME}/storage.h
|
||||
include/${PROJECT_NAME}/subtask.h
|
||||
include/${PROJECT_NAME}/stage.h
|
||||
include/${PROJECT_NAME}/container.h
|
||||
include/${PROJECT_NAME}/task.h
|
||||
include/${PROJECT_NAME}/debug.h
|
||||
)
|
||||
|
||||
add_subdirectory(src/subtasks)
|
||||
add_subdirectory(src/stages)
|
||||
add_subdirectory(src/demo)
|
||||
add_subdirectory(src/test)
|
||||
|
||||
@ -1,17 +1,17 @@
|
||||
#pragma once
|
||||
|
||||
#include "subtask.h"
|
||||
#include "stage.h"
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
class ContainerBasePrivate;
|
||||
class ContainerBase : public SubTask
|
||||
class ContainerBase : public Stage
|
||||
{
|
||||
public:
|
||||
PRIVATE_CLASS(ContainerBase)
|
||||
typedef SubTask::pointer value_type;
|
||||
typedef Stage::pointer value_type;
|
||||
|
||||
typedef std::function<bool(const SubTask&, int depth)> StageCallback;
|
||||
typedef std::function<bool(const Stage&, int depth)> StageCallback;
|
||||
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
|
||||
|
||||
virtual bool canInsert(const value_type& stage, int before = -1) const = 0;
|
||||
|
||||
@ -28,39 +28,39 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
MOVEIT_CLASS_FORWARD(Interface)
|
||||
MOVEIT_CLASS_FORWARD(SubTask)
|
||||
MOVEIT_CLASS_FORWARD(Stage)
|
||||
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||
class InterfaceState;
|
||||
typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair;
|
||||
|
||||
|
||||
class SubTaskPrivate;
|
||||
class SubTask {
|
||||
friend std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
||||
class StagePrivate;
|
||||
class Stage {
|
||||
friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
|
||||
|
||||
public:
|
||||
PRIVATE_CLASS(SubTask)
|
||||
typedef std::unique_ptr<SubTask> pointer;
|
||||
PRIVATE_CLASS(Stage)
|
||||
typedef std::unique_ptr<Stage> pointer;
|
||||
|
||||
~SubTask();
|
||||
~Stage();
|
||||
|
||||
/// initialize stage once before planning
|
||||
virtual bool init(const planning_scene::PlanningSceneConstPtr& scene);
|
||||
const std::string& getName() const;
|
||||
|
||||
protected:
|
||||
/// SubTask can only be instantiated through derived classes
|
||||
SubTask(SubTaskPrivate *impl);
|
||||
/// Stage can only be instantiated through derived classes
|
||||
Stage(StagePrivate *impl);
|
||||
|
||||
protected:
|
||||
// TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor
|
||||
SubTaskPrivate* const pimpl_; // constness guarantees one initial write
|
||||
// TODO: use unique_ptr<StagePrivate> and get rid of destructor
|
||||
StagePrivate* const pimpl_; // constness guarantees one initial write
|
||||
};
|
||||
std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
||||
std::ostream& operator<<(std::ostream &os, const Stage& stage);
|
||||
|
||||
|
||||
class PropagatingEitherWayPrivate;
|
||||
class PropagatingEitherWay : public SubTask {
|
||||
class PropagatingEitherWay : public Stage {
|
||||
public:
|
||||
PRIVATE_CLASS(PropagatingEitherWay)
|
||||
PropagatingEitherWay(const std::string& name);
|
||||
@ -114,7 +114,7 @@ private:
|
||||
|
||||
|
||||
class GeneratorPrivate;
|
||||
class Generator : public SubTask {
|
||||
class Generator : public Stage {
|
||||
public:
|
||||
PRIVATE_CLASS(Generator)
|
||||
Generator(const std::string& name);
|
||||
@ -126,7 +126,7 @@ public:
|
||||
|
||||
|
||||
class ConnectingPrivate;
|
||||
class Connecting : public SubTask {
|
||||
class Connecting : public Stage {
|
||||
public:
|
||||
PRIVATE_CLASS(Connecting)
|
||||
Connecting(const std::string& name);
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/subtask.h>
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
@ -14,7 +14,7 @@ namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface) }
|
||||
namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
|
||||
}
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class CartesianPositionMotion : public PropagatingEitherWay {
|
||||
public:
|
||||
@ -2,9 +2,9 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/subtask.h>
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class CurrentState : public Generator {
|
||||
public:
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/subtask.h>
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
@ -12,7 +12,7 @@ namespace moveit { namespace planning_interface {
|
||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface);
|
||||
} }
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class GenerateGraspPose : public Generator {
|
||||
public:
|
||||
@ -2,13 +2,13 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/subtask.h>
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
|
||||
namespace moveit { namespace planning_interface {
|
||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
||||
} }
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class Gripper : public PropagatingEitherWay {
|
||||
public:
|
||||
@ -2,13 +2,13 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/subtask.h>
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
|
||||
}
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class Move : public Connecting {
|
||||
public:
|
||||
@ -22,7 +22,7 @@ namespace moveit { namespace task_constructor {
|
||||
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||
MOVEIT_CLASS_FORWARD(InterfaceState)
|
||||
MOVEIT_CLASS_FORWARD(Interface)
|
||||
MOVEIT_CLASS_FORWARD(SubTask)
|
||||
MOVEIT_CLASS_FORWARD(Stage)
|
||||
|
||||
|
||||
/** InterfaceState describes a potential start or goal state for a planning stage.
|
||||
|
||||
@ -14,7 +14,7 @@ namespace moveit { namespace core {
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
MOVEIT_CLASS_FORWARD(SubTask)
|
||||
MOVEIT_CLASS_FORWARD(Stage)
|
||||
MOVEIT_CLASS_FORWARD(ContainerBase)
|
||||
MOVEIT_CLASS_FORWARD(Task)
|
||||
|
||||
@ -28,7 +28,7 @@ public:
|
||||
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(Stage::pointer &&stage);
|
||||
using SerialContainer::clear;
|
||||
|
||||
bool plan();
|
||||
|
||||
@ -19,8 +19,8 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
|
||||
return position;
|
||||
}
|
||||
|
||||
inline bool ContainerBasePrivate::canInsert(const SubTask &stage) const {
|
||||
const SubTaskPrivate* impl = stage.pimpl();
|
||||
inline bool ContainerBasePrivate::canInsert(const Stage &stage) const {
|
||||
const StagePrivate* impl = stage.pimpl();
|
||||
return impl->parent() == nullptr // re-parenting is not supported
|
||||
&& impl->trajectories().empty(); // existing trajectories would become invalid
|
||||
}
|
||||
@ -36,17 +36,17 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
|
||||
return true;
|
||||
}
|
||||
|
||||
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask,
|
||||
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&stage,
|
||||
ContainerBasePrivate::const_iterator pos) {
|
||||
SubTaskPrivate *impl = subtask->pimpl();
|
||||
ContainerBasePrivate::iterator it = children_.insert(pos, std::move(subtask));
|
||||
StagePrivate *impl = stage->pimpl();
|
||||
ContainerBasePrivate::iterator it = children_.insert(pos, std::move(stage));
|
||||
impl->setHierarchy(this, it);
|
||||
return it;
|
||||
}
|
||||
|
||||
|
||||
ContainerBase::ContainerBase(ContainerBasePrivate *impl)
|
||||
: SubTask(impl)
|
||||
: Stage(impl)
|
||||
{
|
||||
}
|
||||
PIMPL_FUNCTIONS(ContainerBase)
|
||||
@ -85,7 +85,7 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
|
||||
ends_.reset(new Interface(Interface::NotifyFunction()));
|
||||
}
|
||||
|
||||
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
||||
StagePrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
||||
InterfaceFlags f;
|
||||
if (children().empty()) return f;
|
||||
f |= children().front()->pimpl()->announcedFlags() & INPUT_IF_MASK;
|
||||
@ -93,7 +93,7 @@ SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
||||
return f;
|
||||
}
|
||||
|
||||
inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBasePrivate::const_iterator before) const {
|
||||
inline bool SerialContainerPrivate::canInsert(const Stage &stage, ContainerBasePrivate::const_iterator before) const {
|
||||
if (!ContainerBasePrivate::canInsert(stage))
|
||||
return false;
|
||||
return true;
|
||||
@ -105,24 +105,24 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
|
||||
bool at_begin = (before == children().begin());
|
||||
bool at_end = (before == children().end());
|
||||
|
||||
SubTaskPrivate *cur = stage->pimpl();
|
||||
StagePrivate *cur = stage->pimpl();
|
||||
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
|
||||
if (children().empty()) { // first child inserted
|
||||
cur->setPrevEnds(this->starts());
|
||||
cur->setNextStarts(this->ends());
|
||||
} else if (at_begin) {
|
||||
SubTaskPrivate *next = (*before)->pimpl();
|
||||
StagePrivate *next = (*before)->pimpl();
|
||||
cur->setPrevEnds(this->starts());
|
||||
cur->setNextStarts(next->starts());
|
||||
next->setPrevEnds(cur->ends());
|
||||
} else if (at_end) {
|
||||
SubTaskPrivate *prev = (*this->prev(before))->pimpl();
|
||||
StagePrivate *prev = (*this->prev(before))->pimpl();
|
||||
prev->setNextStarts(cur->starts());
|
||||
cur->setPrevEnds(prev->ends());
|
||||
cur->setNextStarts(this->ends());
|
||||
} else {
|
||||
SubTaskPrivate *prev = (*this->prev(before))->pimpl();
|
||||
SubTaskPrivate *next = (*before)->pimpl();
|
||||
StagePrivate *prev = (*this->prev(before))->pimpl();
|
||||
StagePrivate *next = (*before)->pimpl();
|
||||
prev->setNextStarts(cur->starts());
|
||||
cur->setPrevEnds(prev->ends());
|
||||
cur->setNextStarts(next->starts());
|
||||
@ -152,21 +152,21 @@ SerialContainer::SerialContainer(const std::string &name)
|
||||
{}
|
||||
PIMPL_FUNCTIONS(SerialContainer)
|
||||
|
||||
bool SerialContainer::canInsert(const value_type& subtask, int before) const
|
||||
bool SerialContainer::canInsert(const value_type& stage, int before) const
|
||||
{
|
||||
auto impl = pimpl();
|
||||
return impl->canInsert(*subtask, impl->position(before));
|
||||
return impl->canInsert(*stage, impl->position(before));
|
||||
}
|
||||
|
||||
bool SerialContainer::insert(value_type&& subtask, int before)
|
||||
bool SerialContainer::insert(value_type&& stage, int before)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
|
||||
ContainerBasePrivate::const_iterator where = impl->position(before);
|
||||
if (!impl->canInsert(*subtask, where))
|
||||
if (!impl->canInsert(*stage, where))
|
||||
return false;
|
||||
|
||||
impl->insert(std::move(subtask), where);
|
||||
impl->insert(std::move(stage), where);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -181,7 +181,7 @@ bool SerialContainerPrivate::compute()
|
||||
for(const auto& stage : children()) {
|
||||
if(!stage->pimpl()->canCompute())
|
||||
continue;
|
||||
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;
|
||||
std::cout << "Computing stage '" << stage->getName() << "':" << std::endl;
|
||||
bool success = stage->pimpl()->compute();
|
||||
computed = true;
|
||||
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
||||
|
||||
@ -1,31 +1,31 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/container.h>
|
||||
#include "subtask_p.h"
|
||||
#include "stage_p.h"
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
class ContainerBasePrivate : public SubTaskPrivate
|
||||
class ContainerBasePrivate : public StagePrivate
|
||||
{
|
||||
friend class ContainerBase;
|
||||
|
||||
public:
|
||||
typedef ContainerBase::value_type value_type;
|
||||
typedef SubTaskPrivate::container_type container_type;
|
||||
typedef StagePrivate::container_type container_type;
|
||||
typedef container_type::iterator iterator;
|
||||
typedef container_type::const_iterator const_iterator;
|
||||
|
||||
inline const container_type& children() const { return children_; }
|
||||
const_iterator position(int before = -1) const;
|
||||
|
||||
bool canInsert(const SubTask& stage) const;
|
||||
virtual iterator insert(value_type &&subtask, const_iterator pos);
|
||||
bool canInsert(const Stage& stage) const;
|
||||
virtual iterator insert(value_type &&stage, const_iterator pos);
|
||||
|
||||
bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const;
|
||||
|
||||
protected:
|
||||
ContainerBasePrivate(ContainerBase *me, const std::string &name)
|
||||
: SubTaskPrivate(me, name)
|
||||
: StagePrivate(me, name)
|
||||
{}
|
||||
|
||||
private:
|
||||
@ -38,7 +38,7 @@ public:
|
||||
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
||||
|
||||
InterfaceFlags announcedFlags() const override;
|
||||
inline bool canInsert(const SubTask& stage, const_iterator before) const;
|
||||
inline bool canInsert(const Stage& stage, const_iterator before) const;
|
||||
virtual iterator insert(value_type &&stage, const_iterator before) override;
|
||||
|
||||
bool canCompute() const override;
|
||||
|
||||
@ -2,7 +2,7 @@ add_executable(foo test.cpp)
|
||||
target_link_libraries(foo)
|
||||
|
||||
add_executable(plan_pick_ur5 plan_pick_ur5.cpp)
|
||||
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
||||
|
||||
add_executable(plan_pick_trixi plan_pick_trixi.cpp)
|
||||
target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||
target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit_task_constructor/debug.h>
|
||||
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||
#include <moveit_task_constructor/subtasks/move.h>
|
||||
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
#include <moveit_task_constructor/stages/move.h>
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/CollisionObject.h>
|
||||
@ -43,17 +43,17 @@ int main(int argc, char** argv){
|
||||
|
||||
Task t;
|
||||
|
||||
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
||||
|
||||
{
|
||||
auto move= std::make_unique<subtasks::Gripper>("open gripper");
|
||||
auto move= std::make_unique<stages::Gripper>("open gripper");
|
||||
move->setEndEffector("left_gripper");
|
||||
move->setTo("open");
|
||||
t.add(std::move(move));
|
||||
}
|
||||
|
||||
{
|
||||
auto move= std::make_unique<subtasks::Move>("move to pre-grasp");
|
||||
auto move= std::make_unique<stages::Move>("move to pre-grasp");
|
||||
move->setGroup("left_arm");
|
||||
move->setPlannerId("RRTConnectkConfigDefault");
|
||||
move->setTimeout(8.0);
|
||||
@ -61,7 +61,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto move= std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
||||
auto move= std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
||||
move->setGroup("left_arm");
|
||||
move->setLink("l_gripper_tool_frame");
|
||||
move->setMinMaxDistance(.03, 0.1);
|
||||
@ -74,7 +74,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto gengrasp= std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose");
|
||||
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||
gengrasp->setEndEffector("left_gripper");
|
||||
//gengrasp->setGroup("arm");
|
||||
gengrasp->setLink("l_gripper_tool_frame");
|
||||
@ -86,7 +86,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto move= std::make_unique<subtasks::Gripper>("grasp");
|
||||
auto move= std::make_unique<stages::Gripper>("grasp");
|
||||
move->setEndEffector("left_gripper");
|
||||
move->setTo("closed");
|
||||
move->graspObject("object");
|
||||
@ -94,7 +94,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||
auto move= std::make_unique<stages::CartesianPositionMotion>("lift object");
|
||||
move->setGroup("left_arm");
|
||||
move->setLink("l_gripper_tool_frame");
|
||||
move->setMinMaxDistance(0.03, 0.05);
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit_task_constructor/debug.h>
|
||||
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||
#include <moveit_task_constructor/subtasks/move.h>
|
||||
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
#include <moveit_task_constructor/stages/move.h>
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
@ -41,17 +41,17 @@ int main(int argc, char** argv){
|
||||
|
||||
Task t;
|
||||
|
||||
t.add(std::make_unique<subtasks::CurrentState>("current state"));
|
||||
t.add(std::make_unique<stages::CurrentState>("current state"));
|
||||
|
||||
{
|
||||
auto move = std::make_unique<subtasks::Gripper>("open gripper");
|
||||
auto move = std::make_unique<stages::Gripper>("open gripper");
|
||||
move->setEndEffector("gripper");
|
||||
move->setTo("open");
|
||||
t.add(std::move(std::move(move)));
|
||||
}
|
||||
|
||||
{
|
||||
auto move = std::make_unique<subtasks::Move>("move to pre-grasp");
|
||||
auto move = std::make_unique<stages::Move>("move to pre-grasp");
|
||||
move->setGroup("arm");
|
||||
move->setPlannerId("RRTConnectkConfigDefault");
|
||||
move->setTimeout(8.0);
|
||||
@ -59,7 +59,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto move = std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
||||
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
||||
move->setGroup("arm");
|
||||
move->setLink("s_model_tool0");
|
||||
move->setMinMaxDistance(.03, 0.1);
|
||||
@ -72,7 +72,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto gengrasp = std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose");
|
||||
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||
gengrasp->setEndEffector("gripper");
|
||||
//gengrasp->setGroup("arm");
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
@ -84,7 +84,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto move = std::make_unique<subtasks::Gripper>("grasp");
|
||||
auto move = std::make_unique<stages::Gripper>("grasp");
|
||||
move->setEndEffector("gripper");
|
||||
move->setTo("closed");
|
||||
move->graspObject("object");
|
||||
@ -92,7 +92,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto move = std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||
auto move = std::make_unique<stages::CartesianPositionMotion>("lift object");
|
||||
move->setGroup("arm");
|
||||
move->setLink("s_model_tool0");
|
||||
move->setMinMaxDistance(0.03, 0.05);
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "subtask_p.h"
|
||||
#include "stage_p.h"
|
||||
#include "container_p.h"
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
@ -6,42 +6,42 @@
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
SubTask::SubTask(SubTaskPrivate *impl)
|
||||
Stage::Stage(StagePrivate *impl)
|
||||
: pimpl_(impl)
|
||||
{
|
||||
}
|
||||
|
||||
SubTask::~SubTask()
|
||||
Stage::~Stage()
|
||||
{
|
||||
delete pimpl_;
|
||||
}
|
||||
|
||||
bool SubTask::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
}
|
||||
|
||||
const std::string& SubTask::getName() const {
|
||||
const std::string& Stage::getName() const {
|
||||
return pimpl_->name_;
|
||||
}
|
||||
|
||||
std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
|
||||
std::ostream& operator<<(std::ostream &os, const Stage& stage) {
|
||||
os << *stage.pimpl();
|
||||
return os;
|
||||
}
|
||||
|
||||
template<SubTaskPrivate::InterfaceFlag own, SubTaskPrivate::InterfaceFlag other>
|
||||
const char* direction(const SubTaskPrivate& stage) {
|
||||
SubTaskPrivate::InterfaceFlags f = stage.deducedFlags();
|
||||
template<StagePrivate::InterfaceFlag own, StagePrivate::InterfaceFlag other>
|
||||
const char* direction(const StagePrivate& stage) {
|
||||
StagePrivate::InterfaceFlags f = stage.deducedFlags();
|
||||
bool own_if = f & own;
|
||||
bool other_if = f & other;
|
||||
bool reverse = own & SubTaskPrivate::INPUT_IF_MASK;
|
||||
bool reverse = own & StagePrivate::INPUT_IF_MASK;
|
||||
if (own_if && other_if) return "<>";
|
||||
if (!own_if && !other_if) return "--";
|
||||
if (other_if ^ reverse) return "->";
|
||||
return "<-";
|
||||
};
|
||||
|
||||
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||
std::ostream& operator<<(std::ostream &os, const StagePrivate& stage) {
|
||||
// starts
|
||||
for (const Interface* i : {stage.prev_ends_, stage.starts_.get()}) {
|
||||
os << std::setw(3);
|
||||
@ -49,9 +49,9 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||
else os << "-";
|
||||
}
|
||||
// trajectories
|
||||
os << std::setw(5) << direction<SubTaskPrivate::READS_START, SubTaskPrivate::WRITES_PREV_END>(stage)
|
||||
os << std::setw(5) << direction<StagePrivate::READS_START, StagePrivate::WRITES_PREV_END>(stage)
|
||||
<< std::setw(3) << stage.trajectories_.size()
|
||||
<< std::setw(5) << direction<SubTaskPrivate::READS_END, SubTaskPrivate::WRITES_NEXT_START>(stage);
|
||||
<< std::setw(5) << direction<StagePrivate::READS_END, StagePrivate::WRITES_NEXT_START>(stage);
|
||||
// ends
|
||||
for (const Interface* i : {stage.ends_.get(), stage.next_starts_}) {
|
||||
os << std::setw(3);
|
||||
@ -64,17 +64,17 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||
}
|
||||
|
||||
|
||||
SubTaskPrivate::SubTaskPrivate(SubTask *me, const std::string &name)
|
||||
StagePrivate::StagePrivate(Stage *me, const std::string &name)
|
||||
: me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr)
|
||||
{}
|
||||
|
||||
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
|
||||
SubTrajectory& StagePrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
|
||||
trajectories_.emplace_back(trajectory);
|
||||
SubTrajectory& back = trajectories_.back();
|
||||
return back;
|
||||
}
|
||||
|
||||
SubTaskPrivate::InterfaceFlags SubTaskPrivate::interfaceFlags() const
|
||||
StagePrivate::InterfaceFlags StagePrivate::interfaceFlags() const
|
||||
{
|
||||
InterfaceFlags result = announcedFlags();
|
||||
result &= ~InterfaceFlags(OWN_IF_MASK);
|
||||
@ -83,7 +83,7 @@ SubTaskPrivate::InterfaceFlags SubTaskPrivate::interfaceFlags() const
|
||||
}
|
||||
|
||||
// return the interface flags that can be deduced from the interface
|
||||
inline SubTaskPrivate::InterfaceFlags SubTaskPrivate::deducedFlags() const
|
||||
inline StagePrivate::InterfaceFlags StagePrivate::deducedFlags() const
|
||||
{
|
||||
InterfaceFlags f;
|
||||
if (starts_) f |= READS_START;
|
||||
@ -95,11 +95,11 @@ inline SubTaskPrivate::InterfaceFlags SubTaskPrivate::deducedFlags() const
|
||||
|
||||
|
||||
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
|
||||
: SubTaskPrivate(me, name), dir(dir)
|
||||
: StagePrivate(me, name), dir(dir)
|
||||
{
|
||||
}
|
||||
|
||||
SubTaskPrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
|
||||
StagePrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
|
||||
InterfaceFlags f;
|
||||
if (dir & PropagatingEitherWay::FORWARD)
|
||||
f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
|
||||
@ -187,7 +187,7 @@ PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
|
||||
}
|
||||
|
||||
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
|
||||
: SubTask(impl)
|
||||
: Stage(impl)
|
||||
{
|
||||
initInterface();
|
||||
}
|
||||
@ -292,10 +292,10 @@ bool PropagatingBackward::computeForward(const InterfaceState &from)
|
||||
|
||||
|
||||
GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
|
||||
: SubTaskPrivate(me, name)
|
||||
: StagePrivate(me, name)
|
||||
{}
|
||||
|
||||
SubTaskPrivate::InterfaceFlags GeneratorPrivate::announcedFlags() const {
|
||||
StagePrivate::InterfaceFlags GeneratorPrivate::announcedFlags() const {
|
||||
return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END});
|
||||
}
|
||||
|
||||
@ -309,7 +309,7 @@ bool GeneratorPrivate::compute() {
|
||||
|
||||
|
||||
Generator::Generator(const std::string &name)
|
||||
: SubTask(new GeneratorPrivate(this, name))
|
||||
: Stage(new GeneratorPrivate(this, name))
|
||||
{}
|
||||
PIMPL_FUNCTIONS(Generator)
|
||||
|
||||
@ -326,14 +326,14 @@ void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double co
|
||||
|
||||
|
||||
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
|
||||
: SubTaskPrivate(me, name)
|
||||
: StagePrivate(me, name)
|
||||
{
|
||||
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
|
||||
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
|
||||
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
|
||||
}
|
||||
|
||||
SubTaskPrivate::InterfaceFlags ConnectingPrivate::announcedFlags() const {
|
||||
StagePrivate::InterfaceFlags ConnectingPrivate::announcedFlags() const {
|
||||
return InterfaceFlags({READS_START, READS_END});
|
||||
}
|
||||
|
||||
@ -366,7 +366,7 @@ bool ConnectingPrivate::compute() {
|
||||
|
||||
|
||||
Connecting::Connecting(const std::string &name)
|
||||
: SubTask(new ConnectingPrivate(this, name))
|
||||
: Stage(new ConnectingPrivate(this, name))
|
||||
{
|
||||
}
|
||||
PIMPL_FUNCTIONS(Connecting)
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit_task_constructor/subtask.h>
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
|
||||
// define pimpl() functions accessing correctly casted pimpl_ pointer
|
||||
@ -13,14 +13,14 @@
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
class ContainerBasePrivate;
|
||||
class SubTaskPrivate {
|
||||
friend class SubTask;
|
||||
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||
class StagePrivate {
|
||||
friend class Stage;
|
||||
friend std::ostream& operator<<(std::ostream &os, const StagePrivate& stage);
|
||||
|
||||
public:
|
||||
typedef std::list<SubTask::pointer> container_type;
|
||||
typedef std::list<Stage::pointer> container_type;
|
||||
|
||||
SubTaskPrivate(SubTask* me, const std::string& name);
|
||||
StagePrivate(Stage* me, const std::string& name);
|
||||
|
||||
enum InterfaceFlag {
|
||||
READS_START = 0x01,
|
||||
@ -62,7 +62,7 @@ public:
|
||||
inline void setNextStarts(Interface * next_starts) { next_starts_ = next_starts; }
|
||||
|
||||
protected:
|
||||
SubTask* const me_; // associated/owning SubTask instance
|
||||
Stage* const me_; // associated/owning Stage instance
|
||||
const std::string name_;
|
||||
|
||||
InterfacePtr starts_;
|
||||
@ -77,10 +77,10 @@ private:
|
||||
Interface *prev_ends_; // interface to be used for sendBackward()
|
||||
Interface *next_starts_; // interface to be used for sendForward()
|
||||
};
|
||||
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||
std::ostream& operator<<(std::ostream &os, const StagePrivate& stage);
|
||||
|
||||
|
||||
class PropagatingEitherWayPrivate : public SubTaskPrivate {
|
||||
class PropagatingEitherWayPrivate : public StagePrivate {
|
||||
friend class PropagatingEitherWay;
|
||||
|
||||
public:
|
||||
@ -121,7 +121,7 @@ public:
|
||||
};
|
||||
|
||||
|
||||
class GeneratorPrivate : public SubTaskPrivate {
|
||||
class GeneratorPrivate : public StagePrivate {
|
||||
public:
|
||||
inline GeneratorPrivate(Generator *me, const std::string &name);
|
||||
InterfaceFlags announcedFlags() const override;
|
||||
@ -131,7 +131,7 @@ public:
|
||||
};
|
||||
|
||||
|
||||
class ConnectingPrivate : public SubTaskPrivate {
|
||||
class ConnectingPrivate : public StagePrivate {
|
||||
friend class Connecting;
|
||||
|
||||
public:
|
||||
@ -152,6 +152,6 @@ private:
|
||||
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
||||
};
|
||||
|
||||
PIMPL_FUNCTIONS(SubTask)
|
||||
PIMPL_FUNCTIONS(Stage)
|
||||
|
||||
} }
|
||||
14
src/stages/CMakeLists.txt
Normal file
14
src/stages/CMakeLists.txt
Normal file
@ -0,0 +1,14 @@
|
||||
add_library(${PROJECT_NAME}_stages
|
||||
move.cpp
|
||||
current_state.cpp
|
||||
gripper.cpp
|
||||
generate_grasp_pose.cpp
|
||||
cartesian_position_motion.cpp
|
||||
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/move.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/current_state.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/gripper.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/generate_grasp_pose.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/cartesian_position_motion.h
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}_stages ${catkin_LIBRARIES})
|
||||
@ -1,4 +1,4 @@
|
||||
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
@ -13,7 +13,7 @@
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
||||
: PropagatingEitherWay(name),
|
||||
@ -1,6 +1,6 @@
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
CurrentState::CurrentState(std::string name)
|
||||
: Generator(name)
|
||||
@ -1,4 +1,4 @@
|
||||
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/DisplayRobotState.h>
|
||||
@ -15,7 +15,7 @@
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||
: Generator(name),
|
||||
@ -1,4 +1,4 @@
|
||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit_task_constructor/task.h>
|
||||
|
||||
@ -11,7 +11,7 @@
|
||||
#include <moveit_msgs/MotionPlanRequest.h>
|
||||
#include <moveit_msgs/MotionPlanResponse.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
Gripper::Gripper(std::string name)
|
||||
: PropagatingEitherWay(name)
|
||||
@ -1,4 +1,4 @@
|
||||
#include <moveit_task_constructor/subtasks/move.h>
|
||||
#include <moveit_task_constructor/stages/move.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <moveit_task_constructor/task.h>
|
||||
|
||||
@ -11,7 +11,7 @@
|
||||
#include <moveit_msgs/MotionPlanRequest.h>
|
||||
#include <moveit_msgs/MotionPlanResponse.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
Move::Move(std::string name)
|
||||
: Connecting(name),
|
||||
@ -50,7 +50,7 @@ bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
if(!planner_->generatePlan(from.state, req, res))
|
||||
return false;
|
||||
|
||||
// finish subtask
|
||||
// finish stage
|
||||
connect(from, to, res.trajectory_);
|
||||
|
||||
return true;
|
||||
@ -1,14 +0,0 @@
|
||||
add_library(${PROJECT_NAME}_subtasks
|
||||
move.cpp
|
||||
current_state.cpp
|
||||
gripper.cpp
|
||||
generate_grasp_pose.cpp
|
||||
cartesian_position_motion.cpp
|
||||
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/move.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/current_state.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/gripper.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/generate_grasp_pose.h
|
||||
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/cartesian_position_motion.h
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES})
|
||||
@ -118,7 +118,7 @@ const robot_state::RobotState& Task::getCurrentRobotState() const {
|
||||
}
|
||||
|
||||
void Task::printState(){
|
||||
ContainerBase::StageCallback processor = [](const SubTask& stage, int depth) -> bool {
|
||||
ContainerBase::StageCallback processor = [](const Stage& stage, int depth) -> bool {
|
||||
std::cout << std::string(2*depth, ' ') << stage << std::endl;
|
||||
};
|
||||
traverseStages(processor);
|
||||
|
||||
@ -12,13 +12,13 @@ if (CATKIN_ENABLE_TESTING)
|
||||
endif()
|
||||
|
||||
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
||||
target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||
target_link_libraries(test_plan_current_state ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
||||
|
||||
add_executable(test_plan_gripper test_plan_gripper.cpp)
|
||||
target_link_libraries(test_plan_gripper ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||
target_link_libraries(test_plan_gripper ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
||||
|
||||
add_executable(test_plan_generate_grasp_pose test_plan_generate_grasp_pose.cpp)
|
||||
target_link_libraries(test_plan_generate_grasp_pose ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||
target_link_libraries(test_plan_generate_grasp_pose ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
||||
|
||||
add_executable(test_plan_cartesian_forward test_plan_cartesian_forward.cpp)
|
||||
target_link_libraries(test_plan_cartesian_forward ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||
target_link_libraries(test_plan_cartesian_forward ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
#include <container_p.h>
|
||||
#include <subtask_p.h>
|
||||
#include <stage_p.h>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <initializer_list>
|
||||
@ -44,7 +44,7 @@ protected:
|
||||
void SetUp() {}
|
||||
void TearDown() {}
|
||||
|
||||
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<SubTaskPrivate*> &expected) {
|
||||
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<StagePrivate*> &expected) {
|
||||
size_t num = container->children().size();
|
||||
ASSERT_TRUE(num == expected.size()) << "different list lengths";
|
||||
|
||||
@ -61,7 +61,7 @@ protected:
|
||||
size_t pos = 0;
|
||||
auto exp_it = expected.begin();
|
||||
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) {
|
||||
SubTaskPrivate *child = (*it)->pimpl();
|
||||
StagePrivate *child = (*it)->pimpl();
|
||||
EXPECT_EQ(child, *exp_it) << "wrong order";
|
||||
EXPECT_EQ(child->parent(), container) << "wrong parent";
|
||||
EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution";
|
||||
@ -73,8 +73,8 @@ protected:
|
||||
TEST_F(BaseTest, interfaceFlags) {
|
||||
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
|
||||
EXPECT_EQ(g->pimpl()->interfaceFlags(),
|
||||
SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START,
|
||||
SubTaskPrivate::WRITES_PREV_END}));
|
||||
StagePrivate::InterfaceFlags({StagePrivate::WRITES_NEXT_START,
|
||||
StagePrivate::WRITES_PREV_END}));
|
||||
}
|
||||
|
||||
#define VALIDATE(...) \
|
||||
|
||||
@ -2,8 +2,8 @@
|
||||
|
||||
#include <moveit_task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
@ -12,10 +12,10 @@ int main(int argc, char** argv){
|
||||
|
||||
Task t;
|
||||
|
||||
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
||||
|
||||
{
|
||||
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||
auto move= std::make_unique<stages::CartesianPositionMotion>("lift object");
|
||||
move->setGroup("arm");
|
||||
move->setLink("s_model_tool0");
|
||||
move->setMinMaxDistance(0.05, 0.2);
|
||||
@ -29,7 +29,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
{
|
||||
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||
auto move= std::make_unique<stages::CartesianPositionMotion>("lift object");
|
||||
move->setGroup("arm");
|
||||
move->setLink("s_model_tool0");
|
||||
move->setMinDistance(0.01);
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
#include <moveit_task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
@ -11,7 +11,7 @@ int main(int argc, char** argv){
|
||||
|
||||
Task t;
|
||||
|
||||
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
||||
|
||||
t.plan();
|
||||
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
#include <moveit_task_constructor/task.h>
|
||||
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/CollisionObject.h>
|
||||
@ -36,7 +36,7 @@ int main(int argc, char** argv){
|
||||
|
||||
Task t;
|
||||
|
||||
auto st= std::make_unique<subtasks::GenerateGraspPose>("generate grasp candidates");
|
||||
auto st= std::make_unique<stages::GenerateGraspPose>("generate grasp candidates");
|
||||
|
||||
st->setEndEffector("gripper");
|
||||
//st->setGroup("arm");
|
||||
|
||||
@ -2,8 +2,8 @@
|
||||
|
||||
#include <moveit_task_constructor/task.h>
|
||||
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
@ -14,9 +14,9 @@ int main(int argc, char** argv){
|
||||
|
||||
Task t;
|
||||
|
||||
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
||||
{
|
||||
auto gripper= std::make_unique<subtasks::Gripper>("close gripper");
|
||||
auto gripper= std::make_unique<stages::Gripper>("close gripper");
|
||||
gripper->setEndEffector("gripper");
|
||||
gripper->setTo("closed");
|
||||
t.add(std::move( gripper ));
|
||||
@ -26,12 +26,12 @@ int main(int argc, char** argv){
|
||||
/*TODO currently not implemented in gripper*/
|
||||
/*
|
||||
{
|
||||
auto gripper= std::make_unique<subtasks::Gripper>("close gripper");
|
||||
auto gripper= std::make_unique<stages::Gripper>("close gripper");
|
||||
gripper->setEndEffector("gripper");
|
||||
gripper->setTo("closed");
|
||||
t.add(std::move( gripper ));
|
||||
}
|
||||
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
||||
t.plan();
|
||||
*/
|
||||
return 0;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user