diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index 0a3d51f..1703d66 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -164,11 +164,10 @@ void Node::HandleSubmapList( } void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { + absl::MutexLock locker(&mutex_); if (submap_slices_.empty() || last_frame_id_.empty()) { return; } - - absl::MutexLock locker(&mutex_); auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_); std::unique_ptr msg_ptr = CreateOccupancyGridMsg( painted_slices, resolution_, last_frame_id_, last_timestamp_);