The callback function is triggered just when i publish from terminal

asked 2018-02-17 08:13:40 -0500

Ward Alshalabi gravatar image

I have a node controlling the velocities of a drone till it reaches a specific point , the coordinates of the goal point is published by another node to the "/go_to" topic. so, whenever i publish a new point coordinates the drone will fly from it's own place to the goal position.

the problem: when i publish a new point coordinates to the "go_to" topic using the terminal "rostopic pub -1 /go_to " the code is working fine.but, when i publish a coordinates from another node the code is not working at all."i think the go_to_callback function is not triggered". note: the publisher node working fine because i tried "rostopic echo /go_to" and the new coordinates are there.

the callback function:

void go_to_Callback(const geometry_msgs::Point::ConstPtr& msg){
desired_Pos.x = msg->x;   //get desired x Position
desired_Pos.y = msg->y;   //get desired y Position
desired_Pos.z = msg->z;   //get desired z Position
b=true;                   //enable the flag that there is new goal buplished
goal_re=false;
}

ros::ok code:

while (ros::ok()){

    if(b==true) //if new goal recieved
    {
        b=false;
        //rotate until the drone face the desired new position
while (((abs(ErrorInOri()))>angle_tolerance)&&(abs(getDistance(current_Pos.x,desired_Pos.x,current_Pos.y,desired_Pos.y,current_Pos.z,desired_Pos.z))>distance_tolerance))
{
//vel.linear.x=PidUpdate(0);
vel.linear.z=PidUpdate(2);
vel.angular.z=PidUpdate(1);
vel.linear.x=0;
vel.linear.y=0;
vel.angular.x=0;
vel.angular.y=0;
Vel_pub.publish(vel);   //publish the new velocity to be applied on the Drone   

ros::spinOnce();    
loop_rate.sleep();  //add delay according to the specified loop rate 
 }
    }
if (getDistance(current_Pos.x,desired_Pos.x,current_Pos.y,desired_Pos.y,current_Pos.z,desired_Pos.z)>distance_tolerance)
 {
     //if the drone facing the desired position move forward while you still controlling the yaw until the drone reaches the goal
vel.linear.x=PidUpdate(0);
vel.linear.z=PidUpdate(2);
vel.angular.z=PidUpdate(1);
Vel_pub.publish(vel);   //publish the new velocity to be applied on the Drone   
ros::spinOnce();    
loop_rate.sleep();  //add delay according to the specified loop rate 
 }

else
    {
        //if the drone reached the goal stop there
vel.linear.x=0;
vel.linear.y=0;
vel.linear.z=0; 
vel.angular.x=0;
vel.angular.y=0;
vel.angular.z=0;
Vel_pub.publish(vel);
        //send an empty message to the "/uav_id/goal_reached" Topic.
        if(goal_re==false)
        {
            goal_re=true;
        goal_reached.publish(Empty_msg); 
        }
ros::spinOnce();    
loop_rate.sleep();  //add delay according to the specified loop rate 
    } 
 }
edit retag flag offensive close merge delete