Ask Your Question
0

Callback and if statement always enter

asked 2018-03-30 16:50:30 -0500

cagatayyuruk gravatar image

updated 2018-03-30 16:58:39 -0500

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();
    }
edit retag flag offensive close merge delete

Comments

Which command or publisher are you using to send the target GPS message?

ahendrix gravatar imageahendrix ( 2018-03-30 21:26:11 -0500 )edit

We designed web interface which publish sensor msgs NavsatFix as a desired point. source code

cagatayyuruk gravatar imagecagatayyuruk ( 2018-03-31 04:44:27 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-03-30 20:06:41 -0500

Gayan Brahmanage gravatar image

Hi, The callback function is called every time when it receives new messages. It depends on the rate of the publishing node. In your program, your callback function is called multiple times for the same goal.

The simplest method you can use is->>> remember the last goal in your program and set the new goal if the current goal is different with your previous goal.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-03-30 16:50:30 -0500

Seen: 173 times

Last updated: Mar 30 '18