Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

[costmap] memory out of bounds when costmap do initMaps

Dear all,
I am using costmap and frontier_exploration for my robot autonomous exploration.
Now I encountered a costmap memory of bounds problem when static layer do incomingMap function.
(the error log shows "corrupted double-linked list" or "free(): invalid next size (normal)")

my costmap_2d version: 1.12.13 (2016-08-15)

I add "launch-prefix="valgrind --tool=memcheck --leak-check=full" in my launch file in order to get more detail log.
here is part of log:

==1806== Invalid write of size 1
==1806==    at 0x4FB5AF8: costmap_2d::CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D&, int, int, int, int) (costmap_layer.cpp:85)
==1806==    by 0x13DFBC1F: costmap_2d::StaticLayer::updateCosts(costmap_2d::Costmap2D&, int, int, int, int) (static_layer.cpp:300)
==1806==    by 0x4F5273D: costmap_2d::LayeredCostmap::updateMap(double, double, double) (layered_costmap.cpp:137)
==1806==    by 0x4F59134: costmap_2d::Costmap2DROS::updateMap() (costmap_2d_ros.cpp:429)
==1806==    by 0x4F5894C: costmap_2d::Costmap2DROS::mapUpdateLoop(double) (costmap_2d_ros.cpp:389)
==1806==    by 0x4FA098E: boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>::operator()(costmap_2d::Costmap2DROS*, double) const (mem_fn_template.hpp:165)
==1806==    by 0x4F9F49A: void boost::_bi::list2<boost::_bi::value<costmap_2d::Costmap2DROS*>, boost::_bi::value<double> >::operator()<boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>, boost::_bi::list0>(boost::_bi::type<void>, boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>&, boost::_bi::list0&, int) (bind.hpp:313)
==1806==    by 0x4F9DF04: boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>, boost::_bi::list2<boost::_bi::value<costmap_2d::Costmap2DROS*>, boost::_bi::value<double> > >::operator()() (bind_template.hpp:20)
==1806==    by 0x4F9A7A9: boost::detail::thread_data<boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>, boost::_bi::list2<boost::_bi::value<costmap_2d::Costmap2DROS*>, boost::_bi::value<double> > > >::run() (thread.hpp:117)
==1806==    by 0x66D5A49: ??? (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0)
==1806==    by 0x68E8183: start_thread (pthread_create.c:312)
==1806==    by 0x61E8FFC: clone (clone.S:111)
==1806==  Address 0x16e0b7a0 is 0 bytes after a block of size 135,168 alloc'd
==1806==    at 0x4C2B800: operator new[](unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==1806==    by 0x4F3D071: costmap_2d::Costmap2D::initMaps(unsigned int, unsigned int) (costmap_2d.cpp:71)
==1806==    by 0x4F3D915: costmap_2d::Costmap2D::resizeMap(unsigned int, unsigned int, double, double, double) (costmap_2d.cpp:91)
==1806==    by 0x4F51996: costmap_2d::LayeredCostmap::resizeMap(unsigned int, unsigned int, double, double, double, bool) (layered_costmap.cpp:71)
==1806==    by 0x13DFB26F: costmap_2d::StaticLayer::incomingMap(boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) (static_layer.cpp:182)
==1806==    by 0x13E07895: boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>::operator()(costmap_2d::StaticLayer*, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) const (mem_fn_template.hpp:165)
==1806==    by 0x13E06A03: void boost::_bi::list2<boost::_bi::value<costmap_2d::StaticLayer*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>, boost::_bi::list1<boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&> >(boost::_bi::type<void>, boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>&, boost::_bi::list1<boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>&, int) (bind.hpp:313)
==1806==    by 0x13E05FC1: void boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<costmap_2d::StaticLayer*>, boost::arg<1> > >::operator()<boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> >(boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) (bind_template.hpp:47)
==1806==    by 0x13E05241: boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<costmap_2d::StaticLayer*>, boost::arg<1> > >, void, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) (function_template.hpp:153)
==1806==    by 0x13E07CF6: boost::function1<void, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>::operator()(boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) const (function_template.hpp:767)
==1806==    by 0x13E06CBB: boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const>) (function_template.hpp:153)
==1806==    by 0x13E0B873: boost::function1<void, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> >::operator()(boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const>) const (function_template.hpp:767)
==1806== 
==1806== Invalid read of size 1
==1806==    at 0x147C74DC: virtual_obstacle::VirtualObstacleLayer::UpdateWithMax(costmap_2d::Costmap2D&, int, int, int, int) (virtual_obstacle_layer.cpp:254)
==1806==    by 0x147C5B75: virtual_obstacle::VirtualObstacleLayer::updateCosts(costmap_2d::Costmap2D&, int, int, int, int) (virtual_obstacle_layer.cpp:86)
==1806==    by 0x4F5273D: costmap_2d::LayeredCostmap::updateMap(double, double, double) (layered_costmap.cpp:137)
==1806==    by 0x4F59134: costmap_2d::Costmap2DROS::updateMap() (costmap_2d_ros.cpp:429)
==1806==    by 0x4F5894C: costmap_2d::Costmap2DROS::mapUpdateLoop(double) (costmap_2d_ros.cpp:389)
==1806==    by 0x4FA098E: boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>::operator()(costmap_2d::Costmap2DROS*, double) const (mem_fn_template.hpp:165)
==1806==    by 0x4F9F49A: void boost::_bi::list2<boost::_bi::value<costmap_2d::Costmap2DROS*>, boost::_bi::value<double> >::operator()<boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>, boost::_bi::list0>(boost::_bi::type<void>, boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>&, boost::_bi::list0&, int) (bind.hpp:313)
==1806==    by 0x4F9DF04: boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>, boost::_bi::list2<boost::_bi::value<costmap_2d::Costmap2DROS*>, boost::_bi::value<double> > >::operator()() (bind_template.hpp:20)
==1806==    by 0x4F9A7A9: boost::detail::thread_data<boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::Costmap2DROS, double>, boost::_bi::list2<boost::_bi::value<costmap_2d::Costmap2DROS*>, boost::_bi::value<double> > > >::run() (thread.hpp:117)
==1806==    by 0x66D5A49: ??? (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0)
==1806==    by 0x68E8183: start_thread (pthread_create.c:312)
==1806==    by 0x61E8FFC: clone (clone.S:111)
==1806==  Address 0x16fb43e0 is 0 bytes after a block of size 135,168 alloc'd
==1806==    at 0x4C2B800: operator new[](unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==1806==    by 0x4F3D071: costmap_2d::Costmap2D::initMaps(unsigned int, unsigned int) (costmap_2d.cpp:71)
==1806==    by 0x4F3D915: costmap_2d::Costmap2D::resizeMap(unsigned int, unsigned int, double, double, double) (costmap_2d.cpp:91)
==1806==    by 0x147C5ED3: virtual_obstacle::VirtualObstacleLayer::matchSize() (virtual_obstacle_layer.cpp:107)
==1806==    by 0x4F51B8B: costmap_2d::LayeredCostmap::resizeMap(unsigned int, unsigned int, double, double, double, bool) (layered_costmap.cpp:76)
==1806==    by 0x13DFB26F: costmap_2d::StaticLayer::incomingMap(boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) (static_layer.cpp:182)
==1806==    by 0x13E07895: boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>::operator()(costmap_2d::StaticLayer*, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) const (mem_fn_template.hpp:165)
==1806==    by 0x13E06A03: void boost::_bi::list2<boost::_bi::value<costmap_2d::StaticLayer*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>, boost::_bi::list1<boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&> >(boost::_bi::type<void>, boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>&, boost::_bi::list1<boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>&, int) (bind.hpp:313)
==1806==    by 0x13E05FC1: void boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<costmap_2d::StaticLayer*>, boost::arg<1> > >::operator()<boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> >(boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) (bind_template.hpp:47)
==1806==    by 0x13E05241: boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, costmap_2d::StaticLayer, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<costmap_2d::StaticLayer*>, boost::arg<1> > >, void, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) (function_template.hpp:153)
==1806==    by 0x13E07CF6: boost::function1<void, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&>::operator()(boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&) const (function_template.hpp:767)
==1806==    by 0x13E06CBB: boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<nav_msgs::OccupancyGrid_<std::allocator<void> > const>) (function_template.hpp:153)
==1806==

Costmap initMaps code:

void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
  boost::unique_lock<mutex_t> lock(*access_);

  delete[] costmap_;
  costmap_ = new unsigned char[size_x * size_y];
  if (size_x_ != 0 && size_y_ != 0)
      ROS_ERROR_STREAM("Costmap2D::initMaps 3 :" << (void *)costmap_ << " , " << (void *)&costmap_[0] << ", " << (void *)&costmap_[size_x_ * size_y_ - 1]);
  else
      ROS_ERROR_STREAM("Costmap2D::initMaps 3 :" << (void *)costmap_ << ".");
}

My custom costmap layer code:

void VirtualObstacleLayer::UpdateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
    if (!enabled_)
        return;

    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> master_lock(*(master_grid.getMutex()));
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*layer_mutex);

    unsigned char* master_array = master_grid.getCharMap();
    unsigned int span = master_grid.getSizeInCellsX();

    for (int j = min_j; j < max_j; j++)
    {
        unsigned int it = j * span + min_i;
        for (int i = min_i; i < max_i; i++)
        {
            if (costmap_[it] == NO_INFORMATION)
            {
                it++;
                continue;
            }

            unsigned char old_cost = master_array[it];
            if (old_cost < costmap_[it])
                master_array[it] = costmap_[it];
            it++;
        }
    }

    lock.unlock();
    master_lock.unlock();
}

I thought "boost::unique_lock<mutex_t> lock(*access_);" should prevent this kind of problem.
Does anyone know how to do fix this problem?
Thanks!