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>
namespace moveit { namespace planning_interface {
MOVEIT_CLASS_FORWARD(MoveGroupInterface);
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
} }
namespace moveit { namespace task_constructor { namespace subtasks {

View File

@ -12,8 +12,10 @@
#include <Eigen/Geometry>
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<moveit::core::RobotStatePtr> 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);
}
} } }

View File

@ -1,18 +1,18 @@
#include <moveit_task_constructor/subtasks/current_state.h>
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;
}
} } }

View File

@ -15,8 +15,10 @@
#include <chrono>
#include <functional>
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;
}
} } }

View File

@ -9,42 +9,37 @@
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MotionPlanResponse.h>
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;
}
} } }

View File

@ -9,51 +9,45 @@
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MotionPlanResponse.h>
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<moveit::planning_interface::MoveGroupInterface>(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<InterfaceState&, InterfaceState&> state_pair= fetchStatePair();
@ -78,3 +72,5 @@ moveit::task_constructor::subtasks::Move::compute(){
return true;
}
} } }