From ca6151cacb347e20649fcf4ebe52b823d790b85b Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 6 Mar 2017 19:49:06 +0100 Subject: [PATCH] first implementation of the Move subtask --- .../moveit_task_constructor/subtasks/move.h | 44 ++++++++++ src/plan_pick.cpp | 7 +- src/subtasks/move.cpp | 80 +++++++++++++++++++ 3 files changed, 127 insertions(+), 4 deletions(-) create mode 100644 include/moveit_task_constructor/subtasks/move.h diff --git a/include/moveit_task_constructor/subtasks/move.h b/include/moveit_task_constructor/subtasks/move.h new file mode 100644 index 00000000..ba4404ca --- /dev/null +++ b/include/moveit_task_constructor/subtasks/move.h @@ -0,0 +1,44 @@ +// copyright Michael 'v4hn' Goerner @ 2017 + +#pragma once + +#include + +namespace moveit::planning_interface { +MOVEIT_CLASS_FORWARD(MoveGroupInterface); +} + +namespace moveit::task_constructor::subtasks { + +class Move : public SubTask { +public: + Move(std::string name); + + virtual bool canCompute(); + + virtual bool compute(); + + void setGroup(std::string group); + void setLink(std::string link); + + void setPlannerId(std::string planner); + void setTimeout(double timeout); + + void setFrom(std::string named_target); + void setTo(std::string named_target); + +protected: + std::string group_; + std::string link_; + + double timeout_; + + std::string planner_id_; + + std::string from_named_target_; + std::string to_named_target_; + + moveit::planning_interface::MoveGroupInterfacePtr mgi_; +}; + +} diff --git a/src/plan_pick.cpp b/src/plan_pick.cpp index 3bc73388..b38775c4 100644 --- a/src/plan_pick.cpp +++ b/src/plan_pick.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -25,15 +26,13 @@ int main(int argc, char** argv){ t.addAfter(move); } -/* { - auto move= std::make_shared("move to pre-grasp"); + auto move= std::make_shared("move to pre-grasp"); move->setGroup("arm"); move->setPlannerId("RRTConnectkConfigDefault"); - move->setPlanningTime(5.0); + move->setTimeout(8.0); t.addAfter(move); } -*/ { auto move= std::make_shared("proceed to grasp pose"); diff --git a/src/subtasks/move.cpp b/src/subtasks/move.cpp index e69de29b..a2739bd6 100644 --- a/src/subtasks/move.cpp +++ b/src/subtasks/move.cpp @@ -0,0 +1,80 @@ +#include + +#include +#include +#include + +#include + +#include +#include + +moveit::task_constructor::subtasks::Move::Move(std::string name) +: moveit::task_constructor::SubTask::SubTask(name), + timeout_(5.0) +{} + +void +moveit::task_constructor::subtasks::Move::setGroup(std::string group){ + group_= group; + mgi_= std::make_shared(group_); +} + +void +moveit::task_constructor::subtasks::Move::setLink(std::string link){ + link_= link; +} + +void +moveit::task_constructor::subtasks::Move::setPlannerId(std::string planner){ + planner_id_= planner; +} + +void +moveit::task_constructor::subtasks::Move::setTimeout(double timeout){ + timeout_= timeout; +} + +/* TODO: implement this in compute +void +moveit::task_constructor::subtasks::Move::setFrom(std::string named_target){ + from_named_target_= named_target; +} + +void +moveit::task_constructor::subtasks::Move::setTo(std::string named_target){ + to_named_target_= named_target; +} +*/ + +bool +moveit::task_constructor::subtasks::Move::canCompute(){ + return hasStatePair(); +} + +bool +moveit::task_constructor::subtasks::Move::compute(){ + assert( scene_->getRobotModel() ); + + std::pair state_pair= fetchStatePair(); + + mgi_->setJointValueTarget(state_pair.second.state->getCurrentState()); + if( !planner_id_.empty() ) + mgi_->setPlannerId(planner_id_); + mgi_->setPlanningTime(timeout_); + + ::planning_interface::MotionPlanRequest req; + mgi_->constructMotionPlanRequest(req); + + ros::Duration(4.0).sleep(); + ::planning_interface::MotionPlanResponse res; + if(!planner_->generatePlan(state_pair.first.state, req, res)) + return false; + + // finish subtask + moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_); + trajectory.connectToBeginning(state_pair.first); + trajectory.connectToEnding(state_pair.second); + + return true; +}