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<<""< 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_WS_fin // the meeting point is out of reach
AppCOnd=3;}
};
cout<<"approach case is ="< ( 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 <<"...."<
↧