From f7fe7fc896d94f3b28c5c7778f0e73c85933e063 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 29 Sep 2017 08:36:56 -0700 Subject: [PATCH] use namespace definitions --- .../moveit_task_constructor/subtasks/move.h | 2 +- src/subtasks/cartesian_position_motion.cpp | 59 ++++++++----------- src/subtasks/current_state.cpp | 14 +++-- src/subtasks/generate_grasp_pose.cpp | 41 ++++++------- src/subtasks/gripper.cpp | 31 +++++----- src/subtasks/move.cpp | 34 +++++------ 6 files changed, 80 insertions(+), 101 deletions(-) diff --git a/include/moveit_task_constructor/subtasks/move.h b/include/moveit_task_constructor/subtasks/move.h index 8e7da46d..58efa932 100644 --- a/include/moveit_task_constructor/subtasks/move.h +++ b/include/moveit_task_constructor/subtasks/move.h @@ -5,7 +5,7 @@ #include namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface); +MOVEIT_CLASS_FORWARD(MoveGroupInterface) } } namespace moveit { namespace task_constructor { namespace subtasks { diff --git a/src/subtasks/cartesian_position_motion.cpp b/src/subtasks/cartesian_position_motion.cpp index 8c2f9ae2..7aebb679 100644 --- a/src/subtasks/cartesian_position_motion.cpp +++ b/src/subtasks/cartesian_position_motion.cpp @@ -12,8 +12,10 @@ #include -moveit::task_constructor::subtasks::CartesianPositionMotion::CartesianPositionMotion(std::string name) -: moveit::task_constructor::SubTask::SubTask(name), +namespace moveit { namespace task_constructor { namespace subtasks { + +CartesianPositionMotion::CartesianPositionMotion(std::string name) +: SubTask(name), step_size_(0.005) { ros::NodeHandle nh; @@ -21,51 +23,42 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::CartesianPositionMo ros::Duration(1.0).sleep(); } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::setGroup(std::string group){ +void CartesianPositionMotion::setGroup(std::string group){ group_= group; } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::setLink(std::string link){ +void CartesianPositionMotion::setLink(std::string link){ link_= link; } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::setMinDistance(double distance){ +void CartesianPositionMotion::setMinDistance(double distance){ min_distance_= distance; } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::setMaxDistance(double distance){ +void CartesianPositionMotion::setMaxDistance(double distance){ max_distance_= distance; } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){ +void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){ setMinDistance(min_distance); setMaxDistance(max_distance); } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){ - mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS; +void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){ + mode_= CartesianPositionMotion::MODE_TOWARDS; towards_= towards; } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){ - mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG; +void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){ + mode_= CartesianPositionMotion::MODE_ALONG; along_= along; } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::setCartesianStepSize(double distance){ +void CartesianPositionMotion::setCartesianStepSize(double distance){ step_size_= distance; } -bool -moveit::task_constructor::subtasks::CartesianPositionMotion::canCompute(){ +bool CartesianPositionMotion::canCompute(){ return hasBeginning() || hasEnding(); } @@ -80,16 +73,14 @@ namespace { } } -bool -moveit::task_constructor::subtasks::CartesianPositionMotion::compute(){ +bool CartesianPositionMotion::compute(){ if( hasEnding() ) return _computeFromEnding(); else if( hasBeginning() ) return _computeFromBeginning(); } -bool -moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginning(){ +bool CartesianPositionMotion::_computeFromBeginning(){ assert( hasBeginning() ); moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning(); @@ -110,7 +101,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni std::vector trajectory_steps; 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& link_pose= robot_state.getGlobalLinkTransform(link_); @@ -139,7 +130,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni 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); Eigen::Vector3d direction; tf::vectorMsgToEigen(along_.vector, direction); @@ -184,8 +175,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni return succeeded; } -bool -moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(){ +bool CartesianPositionMotion::_computeFromEnding(){ assert( hasEnding() ); moveit::task_constructor::InterfaceState& ending= fetchStateEnding(); @@ -206,13 +196,13 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding( Eigen::Vector3d direction; switch(mode_){ - case(moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS): + case(CartesianPositionMotion::MODE_TOWARDS): { const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_); direction= link_pose.linear()*Eigen::Vector3d(-1,0,0); } break; - case(moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG): + case(CartesianPositionMotion::MODE_ALONG): { const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id); tf::vectorMsgToEigen(along_.vector, direction); @@ -263,8 +253,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding( } -void -moveit::task_constructor::subtasks::CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){ +void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){ moveit_msgs::DisplayTrajectory dt; robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start); dt.model_id= scene_->getRobotModel()->getName(); @@ -273,3 +262,5 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_publishTrajectory( dt.model_id= start.getRobotModel()->getName(); pub.publish(dt); } + +} } } diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp index 6eab547b..d8e8d649 100644 --- a/src/subtasks/current_state.cpp +++ b/src/subtasks/current_state.cpp @@ -1,18 +1,18 @@ #include -moveit::task_constructor::subtasks::CurrentState::CurrentState(std::string name) -: moveit::task_constructor::SubTask::SubTask(name) +namespace moveit { namespace task_constructor { namespace subtasks { + +CurrentState::CurrentState(std::string name) +: SubTask(name) { ran_= false; } -bool -moveit::task_constructor::subtasks::CurrentState::canCompute(){ +bool CurrentState::canCompute(){ return !ran_; } -bool -moveit::task_constructor::subtasks::CurrentState::compute(){ +bool CurrentState::compute(){ ran_= true; assert( scene_ ); @@ -25,3 +25,5 @@ moveit::task_constructor::subtasks::CurrentState::compute(){ return true; } + +} } } diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index eb8c8213..2b609d2b 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -15,8 +15,10 @@ #include #include -moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name) -: moveit::task_constructor::SubTask::SubTask(name), +namespace moveit { namespace task_constructor { namespace subtasks { + +GenerateGraspPose::GenerateGraspPose(std::string name) +: SubTask(name), timeout_(0.1), angle_delta_(0.1), max_ik_solutions_(0), @@ -31,54 +33,44 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st ros::Duration(1.0).sleep(); } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setGroup(std::string group){ +void GenerateGraspPose::setGroup(std::string group){ group_= group; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setLink(std::string ik_link){ +void GenerateGraspPose::setLink(std::string ik_link){ ik_link_= ik_link; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setEndEffector(std::string eef){ +void GenerateGraspPose::setEndEffector(std::string eef){ eef_= eef; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setGripperGraspPose(std::string pose_name){ +void GenerateGraspPose::setGripperGraspPose(std::string pose_name){ gripper_grasp_pose_= pose_name; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setObject(std::string object){ +void GenerateGraspPose::setObject(std::string object){ object_= object; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setGraspOffset(double offset){ +void GenerateGraspPose::setGraspOffset(double offset){ grasp_offset_= offset; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout){ +void GenerateGraspPose::setTimeout(double timeout){ timeout_= timeout; remaining_time_= timeout; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delta){ +void GenerateGraspPose::setAngleDelta(double delta){ angle_delta_= delta; } -void -moveit::task_constructor::subtasks::GenerateGraspPose::setMaxIKSolutions(uint32_t n){ +void GenerateGraspPose::setMaxIKSolutions(uint32_t n){ max_ik_solutions_= n; } -bool -moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){ +bool GenerateGraspPose::canCompute(){ return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; } @@ -109,8 +101,7 @@ namespace { } } -bool -moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ +bool GenerateGraspPose::compute(){ assert( scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf" ); planning_scene::PlanningScenePtr grasp_scene = scene_->diff(); @@ -190,3 +181,5 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ return false; } + +} } } diff --git a/src/subtasks/gripper.cpp b/src/subtasks/gripper.cpp index 5c73c03b..90df0842 100644 --- a/src/subtasks/gripper.cpp +++ b/src/subtasks/gripper.cpp @@ -9,42 +9,37 @@ #include #include -moveit::task_constructor::subtasks::Gripper::Gripper(std::string name) -: moveit::task_constructor::SubTask::SubTask(name) +namespace moveit { namespace task_constructor { namespace subtasks { + +Gripper::Gripper(std::string name) + : SubTask(name) {} -void -moveit::task_constructor::subtasks::Gripper::setEndEffector(std::string eef){ +void Gripper::setEndEffector(std::string eef){ eef_= eef; } -void -moveit::task_constructor::subtasks::Gripper::setAttachLink(std::string link){ +void Gripper::setAttachLink(std::string link){ attach_link_= link; } -void -moveit::task_constructor::subtasks::Gripper::setFrom(std::string named_target){ +void Gripper::setFrom(std::string named_target){ from_named_target_= named_target; } -void -moveit::task_constructor::subtasks::Gripper::setTo(std::string named_target){ +void Gripper::setTo(std::string named_target){ to_named_target_= named_target; } -void -moveit::task_constructor::subtasks::Gripper::graspObject(std::string grasp_object){ +void Gripper::graspObject(std::string grasp_object){ grasp_object_= grasp_object; } -bool -moveit::task_constructor::subtasks::Gripper::canCompute(){ +bool Gripper::canCompute(){ return hasBeginning(); } -bool -moveit::task_constructor::subtasks::Gripper::compute(){ +bool Gripper::compute(){ assert( scene_->getRobotModel() ); if(!mgi_){ @@ -88,9 +83,11 @@ moveit::task_constructor::subtasks::Gripper::compute(){ } // finish subtask - moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_); + SubTrajectory& trajectory= addTrajectory(res.trajectory_); trajectory.hasBeginning(start); sendForward(trajectory, scene); return true; } + +} } } diff --git a/src/subtasks/move.cpp b/src/subtasks/move.cpp index 570ba098..cb5110ea 100644 --- a/src/subtasks/move.cpp +++ b/src/subtasks/move.cpp @@ -9,51 +9,45 @@ #include #include -moveit::task_constructor::subtasks::Move::Move(std::string name) -: moveit::task_constructor::SubTask::SubTask(name), - timeout_(5.0) +namespace moveit { namespace task_constructor { namespace subtasks { + +Move::Move(std::string name) + : SubTask(name), + timeout_(5.0) {} -void -moveit::task_constructor::subtasks::Move::setGroup(std::string group){ +void Move::setGroup(std::string group){ group_= group; mgi_= std::make_shared(group_); } -void -moveit::task_constructor::subtasks::Move::setLink(std::string link){ +void Move::setLink(std::string link){ link_= link; } -void -moveit::task_constructor::subtasks::Move::setPlannerId(std::string planner){ +void Move::setPlannerId(std::string planner){ planner_id_= planner; } -void -moveit::task_constructor::subtasks::Move::setTimeout(double timeout){ +void Move::setTimeout(double timeout){ timeout_= timeout; } /* TODO: implement this in compute -void -moveit::task_constructor::subtasks::Move::setFrom(std::string named_target){ +void Move::setFrom(std::string named_target){ from_named_target_= named_target; } -void -moveit::task_constructor::subtasks::Move::setTo(std::string named_target){ +void Move::setTo(std::string named_target){ to_named_target_= named_target; } */ -bool -moveit::task_constructor::subtasks::Move::canCompute(){ +bool Move::canCompute(){ return hasStatePair(); } -bool -moveit::task_constructor::subtasks::Move::compute(){ +bool Move::compute(){ assert( scene_->getRobotModel() ); std::pair state_pair= fetchStatePair(); @@ -78,3 +72,5 @@ moveit::task_constructor::subtasks::Move::compute(){ return true; } + +} } }