Callback and if statement always enter

edit

Hello everyone,

I designed this node for getting move base goal for desired GPS point. I inspired from HUSKY's package: outdoor_waypoint_nav, in Kinetic.

I am using flag based programming. One subscriber for getting current GPS point of vehicle another subscriber for desired gps point and when a new message arrived set the flag true, I thought, when a new desired point published, flag set true and enter inside if statement and send goal to move base and set flag to false, this way never enter inside the if statement, until it gets a new goal GPS point.

But when I send a desired point one time, node always re-enter the if statement and always send same goal to the move base again and again. When I try to send second desired point, the move base goal changes but the same things happen again.

As I know when spinOnce() called, one message in queue will deleted and my flag never gonna be true.

What am i thinking wrong?

Thanks.

void targetCallback (sensor_msgs::NavSatFix target)
{       
        longG=target.longitude;
        latiG=target.latitude;

        ROS_INFO("I got new target points");

        flag = true;

        ROS_INFO("flag is :%s", flag?"true":"false");    
}

void currentCallback (sensor_msgs::NavSatFix current)
{

    longC=current.longitude;
    latiC=current.latitude;

    ROS_INFO("I got to current points");
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "gps_waypoint"); //initiate node called gps_waypoint
    ros::NodeHandle n;
    ROS_INFO("Initiated gps_waypoint node");
    MoveBaseClient ac("/move_base", true);
    //construct an action client that we use to communication with the action named move_base.

    //Initiate subscribers
    ros::Subscriber sub = n.subscribe("/rover_gps/waypoint", 10, targetCallback);
    ros::Subscriber sub1 = n.subscribe("/gps/fix", 10, currentCallback);

    ROS_INFO("Initiated gps_waypoint Subscribers");

    //wait for the action server to come up
    while(!ac.waitForServer(ros::Duration(5.0)))
    {
        wait_count++;
        if(wait_count > 3)
        {
            ROS_ERROR("move_base action server did not come up, killing gps_waypoint node...");
            ros::shutdown();
        }
        ROS_INFO("Waiting for the move_base action server to come up");
    }

    ros::Rate r(1); // 1 hz

    while(ros::ok())
    {
        if (flag == true)
        {       
            ROS_INFO("Received current coordinates latitude:%.8f, longitude:%.8f", latiC, longC);
            ROS_INFO("Received goal coordinates latitude:%.8f, longitude:%.8f", latiG, longG);

            //Convert lat/long to utm:
            UTM_point = latLongtoUTM(latiC, longC);
            UTM_next = latLongtoUTM(latiG, longG);

            //Transform UTM to map point in odom frame
            map_point = UTMtoMapPoint(UTM_point);
            map_next = UTMtoMapPoint(UTM_next);

            //Build goal to send to move_base
            move_base_msgs::MoveBaseGoal goal = buildGoal(map_point, map_next); //initiate a move_base_msg called goal

            // Send Goal
            ROS_INFO("Sending goal");
            ac.sendGoal(goal); //push goal to move_base node

            //Wait for result 
            ROS_INFO("Wait for result");
            ac.waitForResult(); //waiting to see if move_base was able to reach goal

            if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                ROS_INFO("Rover has reached its goal!");
                //switch to next waypoint and repeat
            }
            else
            {
                ROS_WARN("All is Well\n");
            }
            ROS_INFO("flag is :%s", flag?"true":"false");

            flag = false;
            ROS_INFO("flag is :%s", flag?"true":"false");
        }
        r.sleep();
        ros::spinOnce();
    }