Sending goals to navigation stack using code

edit

I'll answer in the only way I know how to do this, I suspect there are better/easier ways.

Since you're discussing the navigation stack, I assume you're using movebase. If there is no valid path to the goal, like it is outside of the mapped area (and the mapped area perimeter is continuous), move base will update the status of the goal as aborted.

If the goal pose isn't valid because your cat is sitting on the goal location, move base won't know that until it moves to the goal to find the cat sitting there. Only then will movebase realize there is no valid goal.

This is what I use to monitor the goal state on Jade using C++ classes. It may work out of the box for you, or maybe you need to fix it up. NOTE: This method works for me. I suspect that using the action client this way in a constructor may cause issues if the constructor is called twice. But I don't call it twice, so I never had an issue.

Define the movebase client object

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>     
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

Set up the needed variables

private:
       ros::NodeHandle n; 
       move_base_msgs::MoveBaseGoal goal;
       //tell the action client that we want to spin a thread by default
       MoveBaseClient ac;

In your class you need to start the movebase action client in the constructor initializer:

class my_class
{
    public:

        my_class() : ac("move_base", true)
...

Make sure the movebase is up and running while in the constructor:

 //wait for the action server to come up
 while(!ac.waitForServer(ros::Duration(5.0)))
 {
     ROS_INFO("Waiting for the move_base action server to come up");            
 }      
     ROS_INFO("move_base action server is up");

When you send a goal from your code, register the callback to use on movebase status update:

 // Need boost::bind to pass in the 'this' pointer
 ac.sendGoal(goal, boost::bind(&my_class::doneCb, this, _1), MoveBaseClient::SimpleActiveCallback());

Process the status in your callback.

 void doneCb(const actionlib::SimpleClientGoalState& state)
 {                
    ROS_INFO("DONECB: Finished in state [%s]", state.toString().c_str());        
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
         // do something as goal was reached                   
    }
    if (ac.getState() == actionlib::SimpleClientGoalState::ABORTED)
    {
        // do something as goal was canceled   
    }           
 }