Merge branch 'develop'

CHANGELOG.rst

@@ -2,6 +2,18 @@
 Changelog for package mvsim
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.11.1 (2024-10-17)
+-------------------
+* Great performance improvement for worlds with many (>100) block objects.
+  Terrain elevation query function has been refactored to use a 2D hash-map instead of naively visiting all objects.
+* ROS node: use correct QoS for gridmap publication, and ensure it is published only once.
+* ROS 2: turtlebot demo: Fix RViz wrong camera topic name
+* ROS 2 1robot demo: update rviz config
+* ROS 2: Use correct QoS for (possibly namespaced) /tf & /tf_static
+* FIX: demo_1robot ROS2 launch error (wrong order in listing ros launch arguments)
+* version.h
+* Contributors: Jose Luis Blanco-Claraco
+
 0.11.0 (2024-10-12)
 -------------------
 * docs: add new demo world

modules/simulator/include/mvsim/World.h

@@ -36,6 +36,7 @@
 #include <list>
 #include <map>
 #include <set>
+#include <unordered_map>
 
 #if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF
 // forward declarations:
@@ -594,6 +595,51 @@ class World : public mrpt::system::COutputLogger
 
 	std::mutex simulationStepRunningMtx_;
 
+	// A 2D-hash table of objects
+	struct lut_2d_coordinates_t
+	{
+		int32_t x, y;
+
+		bool operator==(const lut_2d_coordinates_t& o) const noexcept
+		{
+			return (x == o.x && y == o.y);
+		}
+	};
+
+	static lut_2d_coordinates_t xy_to_lut_coords(const mrpt::math::TPoint2Df& p);
+
+	struct LutIndexHash
+	{
+		std::size_t operator()(const lut_2d_coordinates_t& p) const noexcept
+		{
+			// These are the implicit assumptions of the reinterpret cast below:
+			static_assert(sizeof(int32_t) == sizeof(uint32_t));
+			static_assert(offsetof(lut_2d_coordinates_t, x) == 0 * sizeof(uint32_t));
+			static_assert(offsetof(lut_2d_coordinates_t, y) == 1 * sizeof(uint32_t));
+
+			const uint32_t* vec = reinterpret_cast<const uint32_t*>(&p);
+			return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663);
+		}
+		/// k1 < k2? for std::map containers
+		bool operator()(
+			const lut_2d_coordinates_t& k1, const lut_2d_coordinates_t& k2) const noexcept
+		{
+			if (k1.x != k2.x) return k1.x < k2.x;
+			return k1.y < k2.y;
+		}
+	};
+
+	using LUTCache =
+		std::unordered_map<lut_2d_coordinates_t, std::vector<Simulable::Ptr>, LutIndexHash>;
+
+	/// Ensure the cache is built and up-to-date, then return it:
+	const LUTCache& getLUTCacheOfObjects() const;
+
+	mutable LUTCache lut2d_objects_;
+	mutable bool lut2d_objects_is_up_to_date_ = false;
+
+	void internal_update_lut_cache() const;
+
 	/** GUI stuff  */
 	struct GUI
 	{

modules/simulator/include/mvsim/mvsim_version.h

@@ -12,7 +12,7 @@
 // clang-format off
 #define MVSIM_MAJOR_VERSION 0
 #define MVSIM_MINOR_VERSION 11
-#define MVSIM_PATCH_VERSION 0
+#define MVSIM_PATCH_VERSION 1
 
 #define MVSIM_STR_EXP(__A) #__A
 #define MVSIM_STR(__A)  MVSIM_STR_EXP(__A)

modules/simulator/src/World.cpp

@@ -277,13 +277,28 @@ std::set<float> World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) con
 	// Assumption: getListOfSimulableObjectsMtx() is already adquired by all possible call paths?
 	std::set<float> ret;
 
-	for (const auto& [name, obj] : simulableObjects_)
+	// Optimized search for potential objects that influence this query:
+	// 1) world elements: assuming they are few, visit them all.
+	for (const auto& obj : worldElements_)
 	{
-		if (!obj) continue;
-
 		const auto optZ = obj->getElevationAt(worldXY);
 		if (optZ) ret.insert(*optZ);
 	}
+
+	// 2) blocks: by hashed 2D LUT.
+	const World::LUTCache& lut = getLUTCacheOfObjects();
+	const auto lutCoord = xy_to_lut_coords(worldXY);
+	if (auto it = lut.find(lutCoord); it != lut.end())
+	{
+		for (const auto& obj : it->second)
+		{
+			if (!obj) continue;
+			const auto optZ = obj->getElevationAt(worldXY);
+			if (optZ) ret.insert(*optZ);
+		}
+	}
+
+	// if none:
 	if (ret.empty()) ret.insert(.0f);
 
 	return ret;

modules/simulator/src/World_load_xml.cpp

@@ -207,6 +207,7 @@ void World::parse_tag_block(const XmlParserContext& ctx)
 	// <block> entries:
 	Block::Ptr block = Block::factory(this, ctx.node);
 	insertBlock(block);
+	lut2d_objects_is_up_to_date_ = false;
 }
 
 void World::parse_tag_block_class(const XmlParserContext& ctx)

modules/simulator/src/World_simul.cpp

@@ -578,3 +578,38 @@ void World::internal_simul_pre_step_terrain_elevation()
 		}
 	}  // end for object
 }
+
+const World::LUTCache& World::getLUTCacheOfObjects() const
+{
+	if (!lut2d_objects_is_up_to_date_) internal_update_lut_cache();
+
+	return lut2d_objects_;
+}
+
+World::lut_2d_coordinates_t World::xy_to_lut_coords(const mrpt::math::TPoint2Df& p)
+{
+	constexpr float LUT_GRID_SIZE = 4.0;
+	World::lut_2d_coordinates_t c;
+	c.x = static_cast<int32_t>(p.x / LUT_GRID_SIZE);
+	c.y = static_cast<int32_t>(p.y / LUT_GRID_SIZE);
+	return c;
+}
+
+void World::internal_update_lut_cache() const
+{
+	lut2d_objects_is_up_to_date_ = true;
+
+	lut2d_objects_.clear();
+	for (const auto& [name, obj] : blocks_)
+	{
+		std::set<lut_2d_coordinates_t, LutIndexHash> affected_coords;
+		const auto p = obj->getCPose3D();
+		for (const auto& vertex : obj->blockShape())
+		{
+			const auto pt = p.composePoint({vertex.x, vertex.y, .0});
+			const auto c = xy_to_lut_coords(mrpt::math::TPoint2Df(pt.x, pt.y));
+			affected_coords.insert(c);
+		}
+		for (const auto& c : affected_coords) lut2d_objects_[c].push_back(obj);
+	}
+}

mvsim_node_src/include/mvsim/mvsim_node_core.h

@@ -19,8 +19,6 @@
 #include <mvsim/Comms/Server.h>
 #include <mvsim/World.h>
 #include <tf2/LinearMath/Transform.h>
-#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf2_ros/transform_broadcaster.h>
 
 #include <atomic>
 #include <thread>
@@ -40,6 +38,7 @@
 #include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <std_msgs/Bool.h>
+#include <tf2_msgs/TFMessage.h>
 #include <visualization_msgs/MarkerArray.h>
 
 // usings:
@@ -71,6 +70,7 @@ using Msg_CameraInfo = sensor_msgs::CameraInfo;
 #include <rclcpp/time_source.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include "wrapper/publisher_wrapper.h"
@@ -185,16 +185,27 @@ class MVSimNode
 	rclcpp::Clock::SharedPtr clock_;
 #endif
 
-	struct TPubSubPerVehicle
+	struct WorldPubs
 	{
 #if PACKAGE_ROS_VERSION == 1
-		mvsim_node::shared_ptr<ros::Subscriber>
-			sub_cmd_vel;  //!< Subscribers vehicle's "cmd_vel" topic
-
 		/// used for simul_map publication
 		mvsim_node::shared_ptr<ros::Publisher> pub_map_ros;	 //!< Publisher of "simul_map" topic
 		mvsim_node::shared_ptr<ros::Publisher>
 			pub_map_metadata;  //!< Publisher of "simul_map_metadata" topic
+#else
+		/// used for simul_map publication
+		rclcpp::Publisher<Msg_OccupancyGrid>::SharedPtr pub_map_ros;
+		rclcpp::Publisher<Msg_MapMetaData>::SharedPtr pub_map_metadata;
+#endif
+	};
+
+	WorldPubs worldPubs_;
+
+	struct TPubSubPerVehicle
+	{
+#if PACKAGE_ROS_VERSION == 1
+		mvsim_node::shared_ptr<ros::Subscriber>
+			sub_cmd_vel;  //!< Subscribers vehicle's "cmd_vel" topic
 
 		mvsim_node::shared_ptr<ros::Publisher> pub_odom;  //!< Publisher of "odom" topic
 		mvsim_node::shared_ptr<ros::Publisher>
@@ -220,10 +231,6 @@ class MVSimNode
 		/// Subscribers vehicle's "cmd_vel" topic
 		rclcpp::Subscription<Msg_Twist>::SharedPtr sub_cmd_vel;
 
-		/// used for simul_map publication
-		rclcpp::Publisher<Msg_OccupancyGrid>::SharedPtr pub_map_ros;
-		rclcpp::Publisher<Msg_MapMetaData>::SharedPtr pub_map_metadata;
-
 		/// Publisher of "odom" topic
 		rclcpp::Publisher<Msg_Odometry>::SharedPtr pub_odom;
 		/// "base_pose_ground_truth" topic
@@ -328,7 +335,7 @@ class MVSimNode
 
 	mrpt::system::CTimeLogger profiler_{true /*enabled*/, "mvsim_node"};
 
-	void publishWorldElements(mvsim::WorldElementBase& obj, TPubSubPerVehicle& pubsubs);
+	void publishWorldElements(mvsim::WorldElementBase& obj);
 	void publishVehicles(mvsim::VehicleBase& veh);
 
 	void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservation2DRangeScan& obs);

mvsim_node_src/mvsim_node.cpp

@@ -77,6 +77,8 @@ using Msg_Marker = visualization_msgs::Marker;
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #endif
 
+#include <tf2_ros/qos.hpp>	// DynamicBroadcasterQoS(), etc.
+
 // usings:
 using rclcpp::ok;
 
@@ -457,23 +459,13 @@ void MVSimNode::publishVehicles([[maybe_unused]] mvsim::VehicleBase& veh)
 
 // Visitor: World elements
 // ----------------------------------------
-void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj, TPubSubPerVehicle& pubsubs)
+void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj)
 {
 	// GridMaps --------------
-	static mrpt::system::CTicTac lastMapPublished;
-	if (mvsim::OccupancyGridMap* grid = dynamic_cast<mvsim::OccupancyGridMap*>(&obj);
-		grid && lastMapPublished.Tac() > 2.0)
+	if (mvsim::OccupancyGridMap* grid = dynamic_cast<mvsim::OccupancyGridMap*>(&obj); grid)
 	{
-		lastMapPublished.Tic();
-
-		static Msg_OccupancyGrid ros_map;
-		static mvsim::OccupancyGridMap* cachedGrid = nullptr;
-
-		if (cachedGrid != grid)
-		{
-			cachedGrid = grid;
-			mrpt2ros::toROS(grid->getOccGrid(), ros_map);
-		}
+		Msg_OccupancyGrid ros_map;
+		mrpt2ros::toROS(grid->getOccGrid(), ros_map);
 
 #if PACKAGE_ROS_VERSION == 1
 		static size_t loop_count = 0;
@@ -483,8 +475,8 @@ void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj, TPubSubPerVeh
 #endif
 		ros_map.header.stamp = myNow();
 
-		pubsubs.pub_map_ros->publish(ros_map);
-		pubsubs.pub_map_metadata->publish(ros_map.info);
+		worldPubs_.pub_map_ros->publish(ros_map);
+		worldPubs_.pub_map_metadata->publish(ros_map.info);
 
 	}  // end gridmap
 
@@ -509,20 +501,28 @@ void MVSimNode::notifyROSWorldIsUpdated()
 		auto& pubsubs = pubsub_vehicles_[idx];
 
 		initPubSubs(pubsubs, veh);
+	}
 
-#if PACKAGE_ROS_VERSION == 2
-		// In ROS1 latching works so we only need to do this once, here.
-		// In ROS2,latching doesn't work, we must re-publish on a regular basis...
-		static mrpt::system::CTicTac lastMapPublished;
-		if (lastMapPublished.Tac() > 2.0)
-		{
-			lastMapPublished.Tic();
+#if PACKAGE_ROS_VERSION == 1
+	// pub: simul_map, simul_map_metadata
+	worldPubs_.pub_map_ros = mvsim_node::make_shared<ros::Publisher>(
+		n_.advertise<Msg_OccupancyGrid>("simul_map", 1 /*queue len*/, true /*latch*/));
+	worldPubs_.pub_map_metadata = mvsim_node::make_shared<ros::Publisher>(
+		n_.advertise<Msg_MapMetaData>("simul_map_metadata", 1 /*queue len*/, true /*latch*/));
+#else
+	// pub: <VEH>/simul_map, <VEH>/simul_map_metadata
+	// REP-2003: https://ros.org/reps/rep-2003.html
+	// Maps:  reliable transient-local
+	auto qosLatched = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
 
-			mvsim_world_->runVisitorOnWorldElements([this, &pubsubs](mvsim::WorldElementBase& obj)
-													{ publishWorldElements(obj, pubsubs); });
-		}
+	worldPubs_.pub_map_ros = n_->create_publisher<Msg_OccupancyGrid>("simul_map", qosLatched);
+	worldPubs_.pub_map_metadata =
+		n_->create_publisher<Msg_MapMetaData>("simul_map_metadata", qosLatched);
 #endif
-	}
+
+	// Publish maps and static stuff:
+	mvsim_world_->runVisitorOnWorldElements([this](mvsim::WorldElementBase& obj)
+											{ publishWorldElements(obj); });
 }
 
 ros_Time MVSimNode::myNow() const
@@ -584,15 +584,6 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh)
 	pubsubs.pub_tf_static = mvsim_node::make_shared<ros::Publisher>(
 		n_.advertise<Msg_TFMessage>(vehVarName("tf_static", *veh), publisher_history_len_));
 #else
-	// pub: <VEH>/simul_map, <VEH>/simul_map_metadata
-	rclcpp::QoS qosLatched(rclcpp::KeepLast(10));
-	qosLatched.durability(rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
-
-	pubsubs.pub_map_ros =
-		n_->create_publisher<Msg_OccupancyGrid>(vehVarName("simul_map", *veh), qosLatched);
-	pubsubs.pub_map_metadata =
-		n_->create_publisher<Msg_MapMetaData>(vehVarName("simul_map_metadata", *veh), qosLatched);
-
 	// pub: <VEH>/odom
 	pubsubs.pub_odom =
 		n_->create_publisher<Msg_Odometry>(vehVarName("odom", *veh), publisher_history_len_);
@@ -606,12 +597,12 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh)
 		n_->create_publisher<Msg_Bool>(vehVarName("collision", *veh), publisher_history_len_);
 
 	// pub: <VEH>/tf, <VEH>/tf_static
-	rclcpp::QoS qosLatched10(rclcpp::KeepLast(10));
-	qosLatched10.durability(rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+	const auto qos = tf2_ros::DynamicBroadcasterQoS();
+	const auto qos_static = tf2_ros::StaticBroadcasterQoS();
 
-	pubsubs.pub_tf = n_->create_publisher<Msg_TFMessage>(vehVarName("tf", *veh), qosLatched10);
+	pubsubs.pub_tf = n_->create_publisher<Msg_TFMessage>(vehVarName("tf", *veh), qos);
 	pubsubs.pub_tf_static =
-		n_->create_publisher<Msg_TFMessage>(vehVarName("tf_static", *veh), qosLatched10);
+		n_->create_publisher<Msg_TFMessage>(vehVarName("tf_static", *veh), qos_static);
 #endif
 
 	// pub: <VEH>/chassis_markers
@@ -804,17 +795,6 @@ void MVSimNode::spinNotifyROS()
 		// MRPT_TODO("Publish /clock for ROS2 too?");
 #endif
 
-#if PACKAGE_ROS_VERSION == 2
-	// In ROS2,latching doesn't work, we must re-publish on a regular basis...
-	for (size_t idx = 0; idx < vehs.size(); ++idx)
-	{
-		auto& pubs = pubsub_vehicles_[idx];
-
-		mvsim_world_->runVisitorOnWorldElements([this, &pubs](mvsim::WorldElementBase& obj)
-												{ publishWorldElements(obj, pubs); });
-	}
-#endif
-
 	// Publish all TFs for each vehicle:
 	// ---------------------------------------------------------------------
 	if (tim_publish_tf_.Tac() > period_ms_publish_tf_ * 1e-3)

mvsim_tutorial/demo_1robot.launch.py

@@ -54,8 +54,8 @@ def generate_launch_description():
     return LaunchDescription([
         world_file_launch_arg,
         do_fake_localization_arg,
+        headless_launch_arg,
         mvsim_node,
         use_rviz_arg,
         rviz2_node,
-        headless_launch_arg,
     ])