Ask Your Question
0

Local costmap's OccupancyGrid doesn't update origin [closed]

asked 2015-07-15 10:37:48 -0500

BlitherPants gravatar image

updated 2015-07-15 10:38:38 -0500

Hi,

I'm using ROS Indigo on Ubuntu 14.04

I am in the process of writing an autonomous drive algorithm for my Turtlebot, and I've written a function that is meant to search the local costmap for a safe goal to send to the actionclient for move_base.

So, in my function, I call a function that picks up the most recent OccupancyGrid published to move_base/local_costmap/costmap. I then copy the callback to a global variable called "localgrid", which my function examines for obstacles.

void localmapcallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{        
    localgrid.info = msg->info;     
    localgrid.header = msg->header;
    localgrid.data = msg->data;

    gotmap = true;
    ROS_INFO("Got a local map! Origin is %f,%f", msg->info.origin.position.x,msg->info.origin.position.y);
}

//  ^ localmapcallback

What I've noticed, however, is that, no matter where I am in the global map, the origin on the occupancy grid never changes!

Move_base is being launched from the default AMCL demo launch, so I can see that the local costmap is configured properly, i.e., rolling_window is set true. The local costmap also looks fine in RViz.

Also, in my program, I have my own self-updating global Costmap 2DROS named localmap_

localmap_ = new costmap_2d::Costmap2DROS(std::string("newlocal_costmap"), tf_);

So what I have done, is add a lot of debug statements in my "safe goal" function. I compare the difference between the origin of the OccupancyGrid ("localgrid") and the Costmap2DROS ("localmap_") after running the above callback:

ROS_INFO("Looking for a safe goal. The local OG origin is %f,%f",localgrid.info.origin.position.x,localgrid.info.origin.position.y);

costmap_2d::Costmap2D* mymap = localmap_->getCostmap();
boost::unique_lock< boost::recursive_mutex> lock(*(localmap_->getCostmap()->getMutex()));

ROS_INFO("Looking for a safe goal. The local costmap origin is %f,%f",mymap->getOriginX(),mymap->getOriginY());
boost::unique_lock< boost::recursive_mutex> unlock(*(mymap->getMutex()));

And this is what I get when I run the code while the robot is far from its starting point: image description

(Apologies for the photo instead of pasted text -- I can't figure out how to copy output from GDB. Also, apologies for my coding conventions. They're sloppy and I need to fix them eventually)

So, why doesn't the OccupancyGrid have a meaningful origin like the local Costmap2DROS does?

My code is quite long, so that is why I've refrained from posting everything here, but please do let me know if I need to put anything else here.

Thanks!

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2018-01-11 20:21:34.453211

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-07-16 10:43:30 -0500

David Lu gravatar image

Do you see improvement if you set always_send_full_costmap to be true?

One tricky part is that the ROS costmap publishes OccupancyGrid messages and OccupancyGridUpdate messages. However, the OccupancyGridUpdate messages are not useful when the origin changes.

edit flag offensive delete link more

Comments

I already tried that, and no, sadly it doesn't. I wonder if someone should update the wiki with that parameter BTW? I had to search the code to find it. Thanks though!

BlitherPants gravatar image BlitherPants  ( 2015-07-16 11:00:46 -0500 )edit

It would be great if you would update the wiki. I am puzzled by the fact that your costmap is correctly visualized, but the origin is not changing. Can you create a bag file with TF and /move_base/local_costmap/costmap?

David Lu gravatar image David Lu  ( 2015-07-16 11:52:42 -0500 )edit

Oh sorry, I'm more of a beginner and not familiar with wiki editing. I was more just curious if it was something that belonged there. Strange thing is that I couldn't find it listed when I ran rosparam list, so I don't know if it's more of a legacy parameter?

BlitherPants gravatar image BlitherPants  ( 2015-07-16 12:13:11 -0500 )edit

OK, this is very strange. Now when booting up AMCL and driving the robot around manually, I can echo /move_base/local_costmap/costmap and see that the origin is updating. But I echoed it before and it didn't! Maybe it has something to do with the second costmap my drive algorithm creates?

BlitherPants gravatar image BlitherPants  ( 2015-07-16 12:29:50 -0500 )edit

Testing, the origin seems to shift correctly only if I never use"2D Pose estimate" in RViz. The default initial position is [0,0] in the AMCL launch, but if I start anywhere else, I use the button.

BlitherPants gravatar image BlitherPants  ( 2015-07-16 12:56:14 -0500 )edit

(cont'd) When I press the button, it shifts the tf properly (which is why it visualizes correctly?), but the origin doesn't move at all. Driving after that does move the origin, but only as though it had started from [0,0]. Would you still like a bag or is that basically the solution?

BlitherPants gravatar image BlitherPants  ( 2015-07-16 12:58:04 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2015-07-15 10:37:48 -0500

Seen: 918 times

Last updated: Jul 16 '15