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

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 (map_mutex_);
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");
}
/ [...] */
}

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

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