0
$\begingroup$

Rosanswers logo

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=TRGTS.x;  
    Y_t=TRGTS.y;            
    Tstamp=TRGTS.t_s;       
    Labl=TRGTS.label;

    if (Labl==1 || Labl==0)// if labels are not set =0 or it is from the first grp

        {ROS_INFO(" label is %ld -> motion plan does NOT approach the object ", (long int)Labl);}

    else 
    {
        ROS_INFO(" label is %ld -> planing will be considered ", (long int)Labl);
        
        

        TIME_NOW_calc =ros::WallTime::now().toSec();
        X_t=   X_firstRecord + (   V_Co* (   TIME_NOW_calc-Tstamp   )) ; // X position of the target NOW

        V=V_Co/V_Ro; // calculation of the meeting point
        Y_Ofs= Y_t -Y_H ;
        a= 1- (V*V);
        b=  -(2*X_t)-(2*V*V*X_H);   
        c=(X_t*X_t)-(V*V*X_H*X_H)-(Y_Ofs*Y_Ofs);

        //X_MPt= (      -b      - (           pow(      ((b*b) - (4*a*c)) , 0.5      )        )              )/(2*a);
        X_MPt2= (      -b      + (           pow(      ((b*b) - (4*a*c)) , 0.5      )        )              )/(2*a);

        

        //cout<<X_MPt<<endl;// faster
        //cout<<X_MPt2<<endl;
        
        // calculate the CASE
        if ( X_MPt2 < X_WS_St  ){ AppCOnd=1;  }else{ // wait fot the object to arrive 
        if (  X_MPt2 < X_WS_fin   ){ AppCOnd=2; pick_timeWS=( X_MPt2-X_t)/V_Co;}//catch in the workspace   // time to the meeting point time in ws
        else{
        //X_MPt2 > X_WS_fin  // the meeting point is out of reach
        AppCOnd=3;}
        };

        cout<<"approach case is ="<<AppCOnd<<endl;

            moveit::planning_interface::MoveGroup group("ALL_HAND"); //<<---------------??????
            group.setPlanningTime(1);

        
        switch(AppCOnd){// wait for the target to arrive at the border of robot workspace==================
        case(1):


            ROS_INFO(" object to arrive in aprx  %6.4f seconds. current pos is: %6.4f",         (  (X_WS_St-X_t)    /V_Co   ), X_t)  ;
            
            Pick_p_1.position.x= worldY_2_RX(Y_t);// convert the x and y position of the target to the robot pos
            Pick_p_1.position.y= worldX_2_RY(X_WS_St);
            cout<<"moving to border..";group.setPoseTarget(Pick_p_1);group.asyncMove();group.clearPoseTarget();ros::Duration(4).sleep();cout<<"!arrived to border!"<<endl;
                

            if (X_WS_St>  (   X_firstRecord + (   V_Co* (   ros::WallTime::now().toSec()-Tstamp   ))    ) ) // check if we arrived before the meeting point
             {
                cout<<"waiting for for arrival for" << ((X_WS_St-  (   X_firstRecord + (   V_Co* (   ros::WallTime::now().toSec()-Tstamp )) ))) /V_Co <<"...."<<endl;
                ros::Duration(  ((X_WS_St-  (   X_firstRecord + (   V_Co* (   ros::WallTime::now().toSec()-Tstamp )) ))) /V_Co   ).sleep();
            
                cout<<"pick and go to bin....";//PICK

                switch(Labl){
                case 2:group.setPoseTarget(BIN_1);break;
                case 3:group.setPoseTarget(BIN_2);break;
                case 4:group.setPoseTarget(BIN_3);break;
                case 5:group.setPoseTarget(BIN_4);break;}
                group.asyncMove();group.clearPoseTarget();ros::Duration(4).sleep();cout<<"!arrived at bin!"<<endl;// GO TO BIN

                 ros::Duration(  3  ).sleep();

            }else{ROS_ERROR("MIISED THE OBJECT");};

        break;
        case(2):  // catch directly the object in the workspace 
         ROS_INFO(" meeting point in the workspace " )  ;     

            cout<<" pick in work space, x point is:"<<  X_MPt2 <<endl;//move ( X_MPt2,Y_t)
            ros::Duration(  3  ).sleep();
    ;

        break;

        case(3):ROS_WARN(" meeting point out of reach.  %6.4f Meters far", (X_MPt2-X_WS_fin)/1000 );break;// do nothing and reject
        default :ROS_ERROR(" No condition has been identified" );break;

        }// end approach condition switch
        

        

    group.setPoseTarget(HOME_p);group.asyncMove();group.clearPoseTarget();ros::Duration( 4 ).sleep();cout<<"!!arrived at HOME!!"<<endl;// go back home

    };//end of approach based on label
  cout<<"######################end######################"<<endl;
  }//END CALLBACK_TG

};//End of class

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

int main(int argc, char **argv) {

  ROS_INFO(" Decision maker : Ready to process the targets ");
  ros::init(argc, argv, "decision_maker");

  MOPL_class MOplan_obj;

  ros::spin();

  return 0;

}

`


Originally posted by mony.khosravi on ROS Answers with karma: 16 on 2017-12-01

Post score: 0


Original comments

Comment by gvdhoorn on 2017-12-01:
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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

the solution can be found in the similar topic below:

MoveGroup init object issue


Originally posted by mony.khosravi with karma: 16 on 2017-12-04

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.