From 698b0fb18c6be1bb311ccc8300a241398d41e21c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 12 Mar 2024 11:14:55 +0100 Subject: [PATCH] FIX: gridmaps were published at a too high rate in the ROS 2 node --- mvsim_node_src/mvsim_node.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 5b70ab46..3e32666f 100644 --- 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(&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