Ask Your Question
0

Sending goals to navigation stack using code

asked 2017-04-17 11:30:09 -0600

cj gravatar image

Hi,

I think this question is not really related to rtabmap, but I'm hoping that you could point me to the right direction.

I am able to send goals to navigation stack following tutorials on this site http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals. My question is, how can I verify that the location of the goal pose itself is free of obstacles(a valid goal pose)? I'm trying to code up something that will generate valid goal poses by itself.

Thanks, CJ

edit retag flag offensive close merge delete

Comments

How about checking with the obstacle map? for free space?

As an example frontier_exploration does that.

Humpelstilzchen gravatar imageHumpelstilzchen ( 2017-04-18 02:46:31 -0600 )edit

@cj Did you go with the possible solution suggested by Billy? I am in a similar situation where I would like to create a service that I can call to return a viable goal pose. Would be great to hear about your solution if you have decided on an alternative.

surfertas gravatar imagesurfertas ( 2017-05-07 08:37:54 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2017-04-17 12:42:30 -0600

billy gravatar image

updated 2018-11-10 00:33:47 -0600

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   
    }           
 }
edit flag offensive delete link more

Comments

Hi

Thanks for the information, and yes I am using move_base. Correct me if I am wrong: if the goal is outside of the mapped area, is the status of the goal immediately changed to 'ABORTED', and the callback function is immediately called?

cj gravatar imagecj ( 2017-04-17 13:44:46 -0600 )edit

Movebase will spend some time trying to find a path, and will abort before sending any command to move if it cannot develop a global plan that works. There is also a way to request that movebase develop a plan without implementing it. You should read up on that as well.

billy gravatar imagebilly ( 2017-04-17 14:51:49 -0600 )edit

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2017-04-17 11:30:09 -0600

Seen: 1,120 times

Last updated: Nov 10 '18