Local costmap's OccupancyGrid doesn't update origin
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!
Movebase is being launched from the default AMCL demo launch, so I can see that the local costmap is configured properly, i.e., `rollingwindowis 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:
(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!
Asked by BlitherPants on 2015-07-15 10:37:48 UTC
Answers
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.
Asked by David Lu on 2015-07-16 10:43:30 UTC
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!
Asked by BlitherPants on 2015-07-16 11:00:46 UTC
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?
Asked by David Lu on 2015-07-16 11:52:42 UTC
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?
Asked by BlitherPants on 2015-07-16 12:13:11 UTC
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?
Asked by BlitherPants on 2015-07-16 12:29:50 UTC
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.
Asked by BlitherPants on 2015-07-16 12:56:14 UTC
(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?
Asked by BlitherPants on 2015-07-16 12:58:04 UTC
Comments