mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
use namespace definitions
This commit is contained in:
parent
bb1fcad44e
commit
f7fe7fc896
@ -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 {
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user