Threading issue on lock acquire for /dynamic_map service over /scan topic callback

[TamaGO89]

We encountered an issue on big maps:

• start the mapping process
• wait a few minutes
• call /dynamic_map service
• approximately 1 minute wait to receive a response (the waiting time grow the longer the node runs)

We may have found what causes this problem:

rostopic hz /scan ~= 20.0hz

void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) { /* [...] */ // Check the latest timestamp before acquiring the lock if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_) { // Acquire the lock inside updateMap function updateMap(*scan); // Update the latest timestamp after the lock is released last_map_update = scan->header.stamp; ROS_DEBUG("Updated the map"); } /* [...] */ }

updateMap execution time surely greater than 0.05 seconds

void SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan) { ROS_DEBUG("Update map"); // Lock acquired here boost::mutex::scoped_lock map_lock (map_mutex_); /* [...] */ }

This creates a queue of threads waiting on laserCallback to execute updateMap. Making the waiting time for the service /dynamic_map slow, also making the node analize a lot of laser scans without reason.

mapCallback takes a lot of time to evaluate a response, even if it simply paste the latest map into the response bool SlamGMapping::mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { // This callback takes a lot to acquire the lock since there are a lot of laser scans waiting in line boost::mutex::scoped_lock map_lock (map_mutex_); if(got_map_ && map_.map.info.width && map_.map.info.height) { res = map_; return true; } else return false; }

A simple solution would be to move the lock outside the method updateMap ` void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) { / [...] / // Move the lock from updateMap to here boost::mutex::scoped_lock map_lock (mapmutex); if(!gotmap || (scan->header.stamp - last_map_update) > map_updateinterval) { // Acquire the lock inside updateMap function updateMap(scan); // Update the latest timestamp after the lock is released last_map_update = scan->header.stamp; ROS_DEBUG("Updated the map"); } / [...] */ }

void SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan) { ROS_DEBUG("Update map"); // EDIT : remove this lock from here // boost::mutex::scoped_lock map_lock (mapmutex); / [...] / } `

EDIT : I've no idea what this editor is doing, it messed up the layout of the post, i'm sorry