FIX: gridmaps were published at a too high rate in the ROS 2 node

MRPT · Mar 12, 2024 · 698b0fb · 698b0fb
1 parent 4f432a3
commit 698b0fb
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp
@@ -497,10 +497,13 @@ void MVSimNode::publishVehicles([[maybe_unused]] mvsim::VehicleBase& veh)
 void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj)
 {
 	// GridMaps --------------
+	static mrpt::system::CTicTac lastMapPublished;
 	if (mvsim::OccupancyGridMap* grid =
 			dynamic_cast<mvsim::OccupancyGridMap*>(&obj);
-		grid)
+		grid && lastMapPublished.Tac() > 2.0)
 	{
+		lastMapPublished.Tic();
+
 		static Msg_OccupancyGrid ros_map;
 		static mvsim::OccupancyGridMap* cachedGrid = nullptr;
 
@@ -542,11 +545,12 @@ void MVSimNode::notifyROSWorldIsUpdated()
 	static mrpt::system::CTicTac lastMapPublished;
 	if (lastMapPublished.Tac() > 2.0)
 	{
+		lastMapPublished.Tic();
+
 		mvsim_world_->runVisitorOnWorldElements(
 			[this](mvsim::WorldElementBase& obj) {
 				publishWorldElements(obj);
 			});
-		lastMapPublished.Tic();
 	}
 #endif