Merge branch 'develop'

CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package mvsim
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.9.2 (2024-03-21)
+------------------
+* BUG FIX: 3D lidars should not 'see' XYZ corners of wheels
+* BUG FIX: gridmaps were published at a too high rate in the ROS 2 node
+* remove dead code
+* update header version
+* Contributors: Jose Luis Blanco-Claraco
+
 0.9.1 (2024-03-05)
 ------------------
 * Fix use of mrpt bridge to publish XYZIRT point clouds too for ROS 1

README.md

@@ -57,7 +57,7 @@ Build matrix status
 | ROS 1 Noetic (u20.04) | [![Build Status](https://build.ros.org/job/Ndev__mvsim__ubuntu_focal_amd64/badge/icon)](https://build.ros.org/job/Ndev__mvsim__ubuntu_focal_amd64/) |  [![Build Status](https://build.ros.org/job/Nbin_uF64__mvsim__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros.org/job/Nbin_uF64__mvsim__ubuntu_focal_amd64__binary/)  | [![Version](https://img.shields.io/ros/v/noetic/mvsim)](https://index.ros.org/search/?term=mvsim) |
 | ROS 2 Humble (u22.04) | [![Build Status](https://build.ros2.org/job/Hdev__mvsim__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__mvsim__ubuntu_jammy_amd64/) |  [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/) | [![Version](https://img.shields.io/ros/v/humble/mvsim)](https://index.ros.org/search/?term=mvsim) |
 | ROS 2 Iron (u22.04) | [![Build Status](https://build.ros2.org/job/Idev__mvsim__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Idev__mvsim__ubuntu_jammy_amd64/) |  [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mvsim__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mvsim__ubuntu_jammy_amd64__binary/)  | [![Version](https://img.shields.io/ros/v/iron/mvsim)](https://index.ros.org/search/?term=mvsim) |
-| ROS 2 Rolling (u22.04) | [![Build Status](https://build.ros2.org/job/Rdev__mvsim__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Rdev__mvsim__ubuntu_jammy_amd64/) |  [![Build Status](https://build.ros2.org/job/Rbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/)  | [![Version](https://img.shields.io/ros/v/rolling/mvsim)](https://index.ros.org/search/?term=mvsim) |
+| ROS 2 Rolling (u24.04) | [![Build Status](https://build.ros2.org/job/Rdev__mvsim__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Rdev__mvsim__ubuntu_noble_amd64/) |  [![Build Status](https://build.ros2.org/job/Rbin_uN64__mvsim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mvsim__ubuntu_noble_amd64__binary/)  | [![Version](https://img.shields.io/ros/v/rolling/mvsim)](https://index.ros.org/search/?term=mvsim) |
 
 | EOL distro | Stable version |
 | ---    | ---       |

modules/simulator/include/mvsim/VehicleBase.h

@@ -223,8 +223,9 @@ class VehicleBase : public VisualObject, public Simulable
 	// Called from internalGuiUpdate()
 	void internal_internalGuiUpdate_forces(mrpt::opengl::COpenGLScene& scene);
 
-	mrpt::opengl::CSetOfObjects::Ptr glChassis_;
-	std::vector<mrpt::opengl::CSetOfObjects::Ptr> glWheels_;
+	mrpt::opengl::CSetOfObjects::Ptr glChassisViz_, glChassisPhysical_;
+	std::vector<mrpt::opengl::CSetOfObjects::Ptr> glWheelsViz_,
+		glWheelsPhysical_;
 	mrpt::opengl::CSetOfLines::Ptr glForces_;
 	mrpt::opengl::CSetOfLines::Ptr glMotorTorques_;
 

modules/simulator/include/mvsim/Wheel.h

@@ -73,7 +73,7 @@ class Wheel : public VisualObject
 
 	// methods ----
 
-	void getAs3DObject(mrpt::opengl::CSetOfObjects& obj);
+	void getAs3DObject(mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene);
 	void loadFromXML(const rapidxml::xml_node<char>* xml_node);
 
 	void internalGuiUpdate(

modules/simulator/include/mvsim/mvsim_version.h

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

modules/simulator/src/VehicleBase.cpp

@@ -697,18 +697,26 @@ void VehicleBase::internalGuiUpdate(
 	// 1st time call?? -> Create objects
 	// ----------------------------------
 	const size_t nWs = this->getNumWheels();
-	if (!glChassis_ && viz && physical)
+	if (!glChassisViz_ && viz && physical)
 	{
-		glChassis_ = mrpt::opengl::CSetOfObjects::Create();
-		glChassis_->setName("vehicle_chassis_"s + name_);
+		glChassisViz_ = mrpt::opengl::CSetOfObjects::Create();
+		glChassisViz_->setName("vehicle_chassis_"s + name_);
+
+		glChassisPhysical_ = mrpt::opengl::CSetOfObjects::Create();
+		glChassisPhysical_->setName("vehicle_chassis_"s + name_);
 
 		// Wheels shape:
-		glWheels_.resize(nWs);
+		glWheelsViz_.resize(nWs);
+		glWheelsPhysical_.resize(nWs);
 		for (size_t i = 0; i < nWs; i++)
 		{
-			glWheels_[i] = mrpt::opengl::CSetOfObjects::Create();
-			this->getWheelInfo(i).getAs3DObject(*glWheels_[i]);
-			glChassis_->insert(glWheels_[i]);
+			glWheelsViz_[i] = mrpt::opengl::CSetOfObjects::Create();
+			this->getWheelInfo(i).getAs3DObject(*glWheelsViz_[i], false);
+			glChassisViz_->insert(glWheelsViz_[i]);
+
+			glWheelsPhysical_[i] = mrpt::opengl::CSetOfObjects::Create();
+			this->getWheelInfo(i).getAs3DObject(*glWheelsPhysical_[i], true);
+			glChassisPhysical_->insert(glWheelsPhysical_[i]);
 		}
 
 		if (!childrenOnly)
@@ -718,11 +726,12 @@ void VehicleBase::internalGuiUpdate(
 				chassis_poly_, chassis_z_max_ - chassis_z_min_);
 			gl_poly->setLocation(0, 0, chassis_z_min_);
 			gl_poly->setColor_u8(chassis_color_);
-			glChassis_->insert(gl_poly);
+			glChassisViz_->insert(gl_poly);
+			glChassisPhysical_->insert(gl_poly);
 		}
 
-		viz->get().insert(glChassis_);
-		physical->get().insert(glChassis_);
+		viz->get().insert(glChassisViz_);
+		physical->get().insert(glChassisPhysical_);
 	}
 
 	// Update them:
@@ -732,13 +741,18 @@ void VehicleBase::internalGuiUpdate(
 	// need/can't acquire it again:
 	const auto objectPose = viz.has_value() ? getPose() : getPoseNoLock();
 
-	if (glChassis_)
+	if (glChassisViz_)
 	{
-		glChassis_->setPose(objectPose);
+		ASSERT_(glChassisPhysical_);
+
+		glChassisViz_->setPose(objectPose);
+		glChassisPhysical_->setPose(objectPose);
 		for (size_t i = 0; i < nWs; i++)
 		{
 			const Wheel& w = getWheelInfo(i);
-			glWheels_[i]->setPose(mrpt::math::TPose3D(
+			glWheelsPhysical_[i]->setPose(mrpt::math::TPose3D(
+				w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0));
+			glWheelsViz_[i]->setPose(mrpt::math::TPose3D(
 				w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0));
 
 			if (!w.linked_yaw_object_name.empty())
@@ -853,8 +867,8 @@ void VehicleBase::registerOnServer(mvsim::Client& c)
 
 void VehicleBase::chassisAndWheelsVisible(bool visible)
 {
-	if (glChassis_) glChassis_->setVisibility(visible);
-	for (auto& glW : glWheels_)
+	if (glChassisViz_) glChassisViz_->setVisibility(visible);
+	for (auto& glW : glWheelsViz_)
 	{
 		if (glW) glW->setVisibility(visible);
 	}

modules/simulator/src/Wheel.cpp

@@ -24,7 +24,8 @@ using namespace std;
 
 Wheel::Wheel(World* world) : VisualObject(world) { recalcInertia(); }
 
-void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj)
+void Wheel::getAs3DObject(
+	mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene)
 {
 	obj.clear();
 
@@ -40,19 +41,22 @@ void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj)
 		gl_wheel->setPose(
 			mrpt::poses::CPose3D(0, 0.5 * width, 0, 0, 0, mrpt::DEG2RAD(90)));
 
-		auto gl_wheel_frame = mrpt::opengl::CSetOfObjects::Create();
-		gl_wheel_frame->setName("gl_wheel_frame");
-		gl_wheel_frame->insert(gl_wheel);
+		if (!isPhysicalScene)
 		{
-			mrpt::opengl::CSetOfObjects::Ptr gl_xyz =
-				mrpt::opengl::stock_objects::CornerXYZSimple(
-					0.9 * diameter, 2.0);
+			auto gl_wheel_frame = mrpt::opengl::CSetOfObjects::Create();
+			gl_wheel_frame->setName("gl_wheel_frame");
+			gl_wheel_frame->insert(gl_wheel);
+			{
+				mrpt::opengl::CSetOfObjects::Ptr gl_xyz =
+					mrpt::opengl::stock_objects::CornerXYZSimple(
+						0.9 * diameter, 2.0);
 #if MRPT_VERSION >= 0x270
-			gl_xyz->castShadows(false);
+				gl_xyz->castShadows(false);
 #endif
-			gl_wheel_frame->insert(gl_xyz);
+				gl_wheel_frame->insert(gl_xyz);
+			}
+			obj.insert(gl_wheel_frame);
 		}
-		obj.insert(gl_wheel_frame);
 	}
 
 	obj.setPose(mrpt::math::TPose3D(x, y, 0.5 * diameter, yaw, 0.0, 0.0));

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
 
@@ -1539,9 +1543,6 @@ void MVSimNode::internalOn(
 		msg_header.stamp = now;
 		msg_header.frame_id = sSensorFrameId_points;
 
-		mrpt::obs::T3DPointsProjectionParams pp;
-		pp.takeIntoAccountSensorPoseOnRobot = false;
-
 #if defined(HAVE_POINTS_XYZIRT)
 		if (auto* xyzirt = dynamic_cast<const mrpt::maps::CPointsMapXYZIRT*>(
 				obs.pointcloud.get());

package.xml

@@ -5,7 +5,7 @@
   <description>A lightweight multivehicle simulation framework.</description>
 
   <!-- All version numbers in CMake scripts are taken from this line: -->
-  <version>0.9.1</version>
+  <version>0.9.2</version>
 
   <maintainer email="[email protected]">Jose-Luis Blanco-Claraco</maintainer>
   <license>BSD</license>