The callback function is triggered just when i publish from terminal
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
}
}