use namespace definitions

This commit is contained in:
Robert Haschke 2017-09-29 08:36:56 -07:00
parent bb1fcad44e
commit f7fe7fc896
6 changed files with 80 additions and 101 deletions

View File

@ -5,7 +5,7 @@
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/subtask.h>
namespace moveit { namespace planning_interface { namespace moveit { namespace planning_interface {
MOVEIT_CLASS_FORWARD(MoveGroupInterface); MOVEIT_CLASS_FORWARD(MoveGroupInterface)
} } } }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {

View File

@ -12,8 +12,10 @@
#include <Eigen/Geometry> #include <Eigen/Geometry>
moveit::task_constructor::subtasks::CartesianPositionMotion::CartesianPositionMotion(std::string name) namespace moveit { namespace task_constructor { namespace subtasks {
: moveit::task_constructor::SubTask::SubTask(name),
CartesianPositionMotion::CartesianPositionMotion(std::string name)
: SubTask(name),
step_size_(0.005) step_size_(0.005)
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
@ -21,51 +23,42 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::CartesianPositionMo
ros::Duration(1.0).sleep(); ros::Duration(1.0).sleep();
} }
void void CartesianPositionMotion::setGroup(std::string group){
moveit::task_constructor::subtasks::CartesianPositionMotion::setGroup(std::string group){
group_= group; group_= group;
} }
void void CartesianPositionMotion::setLink(std::string link){
moveit::task_constructor::subtasks::CartesianPositionMotion::setLink(std::string link){
link_= link; link_= link;
} }
void void CartesianPositionMotion::setMinDistance(double distance){
moveit::task_constructor::subtasks::CartesianPositionMotion::setMinDistance(double distance){
min_distance_= distance; min_distance_= distance;
} }
void void CartesianPositionMotion::setMaxDistance(double distance){
moveit::task_constructor::subtasks::CartesianPositionMotion::setMaxDistance(double distance){
max_distance_= distance; max_distance_= distance;
} }
void void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
moveit::task_constructor::subtasks::CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
setMinDistance(min_distance); setMinDistance(min_distance);
setMaxDistance(max_distance); setMaxDistance(max_distance);
} }
void void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
moveit::task_constructor::subtasks::CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){ mode_= CartesianPositionMotion::MODE_TOWARDS;
mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS;
towards_= towards; towards_= towards;
} }
void void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
moveit::task_constructor::subtasks::CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){ mode_= CartesianPositionMotion::MODE_ALONG;
mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG;
along_= along; along_= along;
} }
void void CartesianPositionMotion::setCartesianStepSize(double distance){
moveit::task_constructor::subtasks::CartesianPositionMotion::setCartesianStepSize(double distance){
step_size_= distance; step_size_= distance;
} }
bool bool CartesianPositionMotion::canCompute(){
moveit::task_constructor::subtasks::CartesianPositionMotion::canCompute(){
return hasBeginning() || hasEnding(); return hasBeginning() || hasEnding();
} }
@ -80,16 +73,14 @@ namespace {
} }
} }
bool bool CartesianPositionMotion::compute(){
moveit::task_constructor::subtasks::CartesianPositionMotion::compute(){
if( hasEnding() ) if( hasEnding() )
return _computeFromEnding(); return _computeFromEnding();
else if( hasBeginning() ) else if( hasBeginning() )
return _computeFromBeginning(); return _computeFromBeginning();
} }
bool bool CartesianPositionMotion::_computeFromBeginning(){
moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginning(){
assert( hasBeginning() ); assert( hasBeginning() );
moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning(); moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning();
@ -110,7 +101,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni
std::vector<moveit::core::RobotStatePtr> trajectory_steps; std::vector<moveit::core::RobotStatePtr> trajectory_steps;
bool succeeded= false; bool succeeded= false;
if( mode_ == moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS ){ if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
const Eigen::Affine3d& frame= beginning.state->getFrameTransform(towards_.header.frame_id); const Eigen::Affine3d& frame= beginning.state->getFrameTransform(towards_.header.frame_id);
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_); const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
@ -139,7 +130,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni
succeeded= achieved_distance >= min_distance_; succeeded= achieved_distance >= min_distance_;
} }
else if( mode_ == moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG ){ else if( mode_ == CartesianPositionMotion::MODE_ALONG ){
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id); const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
Eigen::Vector3d direction; Eigen::Vector3d direction;
tf::vectorMsgToEigen(along_.vector, direction); tf::vectorMsgToEigen(along_.vector, direction);
@ -184,8 +175,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni
return succeeded; return succeeded;
} }
bool bool CartesianPositionMotion::_computeFromEnding(){
moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(){
assert( hasEnding() ); assert( hasEnding() );
moveit::task_constructor::InterfaceState& ending= fetchStateEnding(); moveit::task_constructor::InterfaceState& ending= fetchStateEnding();
@ -206,13 +196,13 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(
Eigen::Vector3d direction; Eigen::Vector3d direction;
switch(mode_){ switch(mode_){
case(moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS): case(CartesianPositionMotion::MODE_TOWARDS):
{ {
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_); const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
direction= link_pose.linear()*Eigen::Vector3d(-1,0,0); direction= link_pose.linear()*Eigen::Vector3d(-1,0,0);
} }
break; break;
case(moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG): case(CartesianPositionMotion::MODE_ALONG):
{ {
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id); const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
tf::vectorMsgToEigen(along_.vector, direction); tf::vectorMsgToEigen(along_.vector, direction);
@ -263,8 +253,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(
} }
void void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
moveit::task_constructor::subtasks::CartesianPositionMotion::_publishTrajectory(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();
@ -273,3 +262,5 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_publishTrajectory(
dt.model_id= start.getRobotModel()->getName(); dt.model_id= start.getRobotModel()->getName();
pub.publish(dt); pub.publish(dt);
} }
} } }

View File

@ -1,18 +1,18 @@
#include <moveit_task_constructor/subtasks/current_state.h> #include <moveit_task_constructor/subtasks/current_state.h>
moveit::task_constructor::subtasks::CurrentState::CurrentState(std::string name) namespace moveit { namespace task_constructor { namespace subtasks {
: moveit::task_constructor::SubTask::SubTask(name)
CurrentState::CurrentState(std::string name)
: SubTask(name)
{ {
ran_= false; ran_= false;
} }
bool bool CurrentState::canCompute(){
moveit::task_constructor::subtasks::CurrentState::canCompute(){
return !ran_; return !ran_;
} }
bool bool CurrentState::compute(){
moveit::task_constructor::subtasks::CurrentState::compute(){
ran_= true; ran_= true;
assert( scene_ ); assert( scene_ );
@ -25,3 +25,5 @@ moveit::task_constructor::subtasks::CurrentState::compute(){
return true; return true;
} }
} } }

View File

@ -15,8 +15,10 @@
#include <chrono> #include <chrono>
#include <functional> #include <functional>
moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name) namespace moveit { namespace task_constructor { namespace subtasks {
: moveit::task_constructor::SubTask::SubTask(name),
GenerateGraspPose::GenerateGraspPose(std::string name)
: SubTask(name),
timeout_(0.1), timeout_(0.1),
angle_delta_(0.1), angle_delta_(0.1),
max_ik_solutions_(0), max_ik_solutions_(0),
@ -31,54 +33,44 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st
ros::Duration(1.0).sleep(); ros::Duration(1.0).sleep();
} }
void void GenerateGraspPose::setGroup(std::string group){
moveit::task_constructor::subtasks::GenerateGraspPose::setGroup(std::string group){
group_= group; group_= group;
} }
void void GenerateGraspPose::setLink(std::string ik_link){
moveit::task_constructor::subtasks::GenerateGraspPose::setLink(std::string ik_link){
ik_link_= ik_link; ik_link_= ik_link;
} }
void void GenerateGraspPose::setEndEffector(std::string eef){
moveit::task_constructor::subtasks::GenerateGraspPose::setEndEffector(std::string eef){
eef_= eef; eef_= eef;
} }
void void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
moveit::task_constructor::subtasks::GenerateGraspPose::setGripperGraspPose(std::string pose_name){
gripper_grasp_pose_= pose_name; gripper_grasp_pose_= pose_name;
} }
void void GenerateGraspPose::setObject(std::string object){
moveit::task_constructor::subtasks::GenerateGraspPose::setObject(std::string object){
object_= object; object_= object;
} }
void void GenerateGraspPose::setGraspOffset(double offset){
moveit::task_constructor::subtasks::GenerateGraspPose::setGraspOffset(double offset){
grasp_offset_= offset; grasp_offset_= offset;
} }
void void GenerateGraspPose::setTimeout(double timeout){
moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout){
timeout_= timeout; timeout_= timeout;
remaining_time_= timeout; remaining_time_= timeout;
} }
void void GenerateGraspPose::setAngleDelta(double delta){
moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delta){
angle_delta_= delta; angle_delta_= delta;
} }
void void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
moveit::task_constructor::subtasks::GenerateGraspPose::setMaxIKSolutions(uint32_t n){
max_ik_solutions_= n; max_ik_solutions_= n;
} }
bool bool GenerateGraspPose::canCompute(){
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
} }
@ -109,8 +101,7 @@ namespace {
} }
} }
bool bool GenerateGraspPose::compute(){
moveit::task_constructor::subtasks::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();
@ -190,3 +181,5 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
return false; return false;
} }
} } }

View File

@ -9,42 +9,37 @@
#include <moveit_msgs/MotionPlanRequest.h> #include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MotionPlanResponse.h> #include <moveit_msgs/MotionPlanResponse.h>
moveit::task_constructor::subtasks::Gripper::Gripper(std::string name) namespace moveit { namespace task_constructor { namespace subtasks {
: moveit::task_constructor::SubTask::SubTask(name)
Gripper::Gripper(std::string name)
: SubTask(name)
{} {}
void void Gripper::setEndEffector(std::string eef){
moveit::task_constructor::subtasks::Gripper::setEndEffector(std::string eef){
eef_= eef; eef_= eef;
} }
void void Gripper::setAttachLink(std::string link){
moveit::task_constructor::subtasks::Gripper::setAttachLink(std::string link){
attach_link_= link; attach_link_= link;
} }
void void Gripper::setFrom(std::string named_target){
moveit::task_constructor::subtasks::Gripper::setFrom(std::string named_target){
from_named_target_= named_target; from_named_target_= named_target;
} }
void void Gripper::setTo(std::string named_target){
moveit::task_constructor::subtasks::Gripper::setTo(std::string named_target){
to_named_target_= named_target; to_named_target_= named_target;
} }
void void Gripper::graspObject(std::string grasp_object){
moveit::task_constructor::subtasks::Gripper::graspObject(std::string grasp_object){
grasp_object_= grasp_object; grasp_object_= grasp_object;
} }
bool bool Gripper::canCompute(){
moveit::task_constructor::subtasks::Gripper::canCompute(){
return hasBeginning(); return hasBeginning();
} }
bool bool Gripper::compute(){
moveit::task_constructor::subtasks::Gripper::compute(){
assert( scene_->getRobotModel() ); assert( scene_->getRobotModel() );
if(!mgi_){ if(!mgi_){
@ -88,9 +83,11 @@ moveit::task_constructor::subtasks::Gripper::compute(){
} }
// finish subtask // finish subtask
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_); SubTrajectory& trajectory= addTrajectory(res.trajectory_);
trajectory.hasBeginning(start); trajectory.hasBeginning(start);
sendForward(trajectory, scene); sendForward(trajectory, scene);
return true; return true;
} }
} } }

View File

@ -9,51 +9,45 @@
#include <moveit_msgs/MotionPlanRequest.h> #include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MotionPlanResponse.h> #include <moveit_msgs/MotionPlanResponse.h>
moveit::task_constructor::subtasks::Move::Move(std::string name) namespace moveit { namespace task_constructor { namespace subtasks {
: moveit::task_constructor::SubTask::SubTask(name),
timeout_(5.0) Move::Move(std::string name)
: SubTask(name),
timeout_(5.0)
{} {}
void void Move::setGroup(std::string group){
moveit::task_constructor::subtasks::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 void Move::setLink(std::string link){
moveit::task_constructor::subtasks::Move::setLink(std::string link){
link_= link; link_= link;
} }
void void Move::setPlannerId(std::string planner){
moveit::task_constructor::subtasks::Move::setPlannerId(std::string planner){
planner_id_= planner; planner_id_= planner;
} }
void void Move::setTimeout(double timeout){
moveit::task_constructor::subtasks::Move::setTimeout(double timeout){
timeout_= timeout; timeout_= timeout;
} }
/* TODO: implement this in compute /* TODO: implement this in compute
void void Move::setFrom(std::string named_target){
moveit::task_constructor::subtasks::Move::setFrom(std::string named_target){
from_named_target_= named_target; from_named_target_= named_target;
} }
void void Move::setTo(std::string named_target){
moveit::task_constructor::subtasks::Move::setTo(std::string named_target){
to_named_target_= named_target; to_named_target_= named_target;
} }
*/ */
bool bool Move::canCompute(){
moveit::task_constructor::subtasks::Move::canCompute(){
return hasStatePair(); return hasStatePair();
} }
bool bool Move::compute(){
moveit::task_constructor::subtasks::Move::compute(){
assert( scene_->getRobotModel() ); assert( scene_->getRobotModel() );
std::pair<InterfaceState&, InterfaceState&> state_pair= fetchStatePair(); std::pair<InterfaceState&, InterfaceState&> state_pair= fetchStatePair();
@ -78,3 +72,5 @@ moveit::task_constructor::subtasks::Move::compute(){
return true; return true;
} }
} } }