Problem with global costmap after version 1.16.3 update

[PSotiriadis]

After update NavigationStack from V1.16.2 to 1.16.3, a problem occurs in case of resize map.
I tried to find which change in navigation stack maybe produce this problem and I found that it is in costmap_2d package and specially in static_layer.cpp file. The change is on StaticLayer::incomingMap() in “if/else if” control.

In V1.16.2 the code was:

and in V1.16.3 (actual version) the code is:

The layered_costmap_->resizeMap (in layered_costmap.cpp) function is:

void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
                               double origin_y, bool size_locked)
{
  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
  size_locked_ = size_locked;
  costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    (*plugin)->matchSize();
  }
}

As we can see, the problem is that in version V1.16.3 the isSizedLocked(), at the first time, returns false (so is defined as default value in layered_costmap.h) and is able to call layered_costmap_->resizeMap, in which the protected sized_locked_ variable is set to true and is never changed again. That means that we cannot resize the layered costmap again and we can make only static_layer resize. That destroys the global costmap and as result makes the robot unable to drive and plan new paths.

Using Gmapping, defining as global_costmap a big enough map (100mx100m for example) and driving only in this map, no problem occurs. But if the robot explores more than this map size then the static layer is resized but not the costmap with all other layers and that corrupts the global_costmap .

Using Cartographer the problem is bigger because we cannot predefine the map size.

My search for layered_costmap_->resizemap() call in navigation-melodic-devel has only two hits:

1. In static_layer.cpp line 184
2. In costmap_2d_ros.cpp in Costmap2DROS::reconfigureCB() function in line 344 (resize happens only by dynamic reconfigure)

That means that costmap resize happens only in static_layer.cpp and in my opinion there is no reason to use sized_locked_ variable, in this “if/else if” control, because it’s the same if we have fix map size or not to resize the map. If the incoming map is bigger, then we need to resize the layered costmap independent of size_locked_ variable. This “if/else if” should control only if we use rolling window, the size, resolution and origin of the map. Everything was working properly with V1.16.2, because the layered_costmap_->isSizedLocked() was ignored, but using V1.16.3 is not. After removing "!layered_costmap_->isSizeLocked() &&" from if statement everything is working properly, as in V1.16.2.

To indicate the problem I made some screenshots (costmap with points is global_costmap):

• USING GMAPPING
  
  
  

• USING CARTOGRAPHER
  
  
  

My configurations are:

• costmap_common_params.yaml
  

• global_costmap_params.yaml 
  

• local_costmap_params.yaml
  

• move_base.launch
  

[SteveMacenski]

@ghost @aaronhoy @DLu can you respond to these issues from #792?

Edit: Ah. Ghost is not a real person 😉

[joekeo]

I am having the same issue here

[msadowski]

I'm working with @joekeo on this one and we can confirm that after downgrading the package version to 1.16.2 the issue is not there.

Here is how it looked on our side with slam_toolbox:

Any chance we could revert #792 followed by a quick release?

[mikeferguson]

Ok, digging into this:

• layered_costmap.h defines size_locked_ (as private), returns it in isSIzeLocked()
• layered_costmap.cpp sets size_locked_ to False at start. The only other way it changes is through a call to resizeMap() (which has a default value of false)
• LayeredCostmap->resizeMap() is only called in two places: the static layer function where this is, and the reconfigureCallback of Costmap2dROS.
• isSizeLocked() is called in only two places: this call, and in the Costmap2dROS->reconfigureCb

So, as far as I can tell, the sole purpose of isSizeLocked() is to prevent the reconfigure callback from changing the size of the costmap when we are using a static layer. @DLu -- does that seem right?

I propose we fix this with: #966

[SteveMacenski]

That makes sense to me.

[mikeferguson]

I've merged this fix - I'll cut a new release later this week (after I get a chance to do a quick test of everything)

[mirzashah]

@mikeferguson -- this regression was also backported into kinetic in June and introduced with the release of 1.14.5 in November. I can confirm removing that one commit fixes the problem.

69ff9ba

[mikeferguson]

@mirzashah thanks for point that out - fix will be backported in #972 ad released as 1.14.7 (already released 1.14.6 before I saw this)

[derektan95]

Hi @mikeferguson ,

It appears that this issue still persist in the ROS kinetic binaries. I installed the Navigation Stack via sudo apt-get install ros-kinetic-navigation.

Whenever my static global costmap is resized, black blotches appear across the entire map. May I know if these fix have yet to be updated on the binaries?

Looking forward to hearing from you. Thanks.

[mikeferguson]

@derektan95 As you'll see in my comments above, this was fixed in 1.14.7 - http://repositories.ros.org/status_page/ros_kinetic_default.html shows that navigation is 1.14.9 in Kinetic.

[derektan95]

Hi @mikeferguson ,

Thanks for letting me know. I thought it was really strange because the exact same issue as the one mentioned on this thread resurfaced again despite the update. Here are some images of the problem, which occurs whenever the Global Costmap is resized. These black blotches makes navigation impossible.

Not sure if there's some additional bug. Let me know if you have any insights, thanks.