Ask Your Question
0

proper method to use moveit::planning_interface::MoveGroup::move () action in sequnce ?

asked 2017-12-01 04:43:05 -0600

mony.khosravi gravatar image

Hello everyone,

I am currently developing a sorting system using a Hyper-spectral camera, a conveyor belt and a KUKA KR6.R900.

I have already developed the image processing and classification nodes to publish the target messages containing position, label and time stamps of the objects when recorded. With the mentioned information, I have manged to compose a decision making node winch calculates the possible meeting point of the objects moving on the conveyor belt and the robot end effector.

the problem starts when I try to use the move() action to send commands to the simulator. I have no idea that what must the right way to send sequential move() or .asyncMove() actions in a callback function.

Current, the ROS node moves the robot to the desire position for the first target and finishes the task.But for the next objects, it randomly skips the execution, as soon as it arrives to the line moveit::planning_interface::MoveGroup group("ALL_HAND");

I was wondering if I am overloading the action server ?

below you can find the decision making code,

Thank you all in advance

`

// ======================================================================================================= class MOPL_class {

private:

ros::NodeHandle n_; 
ros::Subscriber sub_;

double X_firstRecord; 
double X_t; // target x position (x is along the belt)
double Y_t;
double X_MPt; // calculated meeting point
double X_MPt2;
double Tstamp;
int Labl;
int AppCOnd;
double a;
double b;
double c;
double V;
double Y_Ofs;
double TIME_NOW_calc;
double pick_timeWS;

geometry_msgs::Pose HOME_p;

geometry_msgs::Pose BIN_1; // container position
geometry_msgs::Pose BIN_2;
geometry_msgs::Pose BIN_3;
geometry_msgs::Pose BIN_4;

geometry_msgs::Pose Pick_p_1; // pick point for case 1 
geometry_msgs::Pose Pick_p_2; // for case 2




public: /// Constructor   
  MOPL_class()
  {

    //Listen to the targets coming from the image processor
    sub_ = n_.subscribe("targets_topic", 10000, &MOPL_class::callback_TRG, this);



    HOME_p.position.x = 0.55;//  
    HOME_p.position.y = 0;//  
    HOME_p.position.z = 0.58;
    HOME_p.orientation.x = 0;//  
    HOME_p.orientation.y = 1;
    HOME_p.orientation.z = 0;
    HOME_p.orientation.w = 0;


    BIN_1.position.x = 0.55;// 
    BIN_1.position.y = -0.3;//  
    BIN_1.position.z = 0.58;
    BIN_1.orientation.x = 0;//  
    BIN_1.orientation.y = 1;
    BIN_1.orientation.z = 0;
    BIN_1.orientation.w = 0;

    BIN_2.position.x = 0.8;//  
    BIN_2.position.y = 0;// 
    BIN_2.position.z = 0.58;
    BIN_2.orientation.x = 0;//  
    BIN_2.orientation.y = 1;
    BIN_2.orientation.z = 0;
    BIN_2.orientation.w = 0;

    BIN_3.position.x = 0.37;//  
    BIN_3.position.y = 0;// 
    BIN_3.position.z = 0.58;
    BIN_3.orientation.x = 0;//  
    BIN_3.orientation.y = 1;
    BIN_3.orientation.z = 0;
    BIN_3.orientation.w = 0;

    BIN_4.position.x = 0.37;//  
    BIN_4.position.y = 0;//  
    BIN_4.position.z = 0.58;
    BIN_4.orientation.x = 0;//  
    BIN_4.orientation.y = 1;
    BIN_4.orientation.z = 0;
    BIN_4.orientation.w = 0;

    //Pick_p_1.position.x 
    //Pick_p_1.position.y 
    Pick_p_1.position.z = 0.515;
    Pick_p_1.orientation.x = 0;//  
    Pick_p_1.orientation.y = 1;
    Pick_p_1.orientation.z = 0;
    Pick_p_1.orientation.w = 0;

    //Pick_p_2.position.x 
    //Pick_p_2.position.y 
    Pick_p_2.position.z = 0.515;
    Pick_p_2.orientation.x = 0;//  
    Pick_p_2.orientation.y = 1;
    Pick_p_2.orientation.z = 0;
    Pick_p_2.orientation.w = 0;





  }

  void callback_TRG(const re4_draft::targets& TRGTS) //=========================
  {

    cout<<""<<endl;
    cout<<"########################start####################"<<endl;

    X_firstRecord ...
(more)
edit retag flag offensive close merge delete

Comments

First thing: don't create the MoveGroup instance inside a callback. Make it a class member.

That's almost generally applicable btw: try to avoid constructing objects that are costly to initialise and / or have a lot pubs/subs/svc clients themselves in callbacks.

gvdhoorn gravatar imagegvdhoorn ( 2017-12-01 04:51:52 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-12-04 09:32:22 -0600

mony.khosravi gravatar image

the solution can be found in the similar topic below:

MoveGroup init object issue

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-12-01 04:43:05 -0600

Seen: 234 times

Last updated: Dec 04 '17