Merge branch 'develop'

appveyor.yml

@@ -1,5 +1,5 @@
 # version format
-version: 2.11.10-{branch}-build{build}
+version: 2.11.11-{branch}-build{build}
 
 os: Visual Studio 2019
 

doc/source/doxygen-docs/changelog.md

@@ -1,6 +1,13 @@
 \page changelog Change Log
 
-# Version 2.11.10: Relased Feb 19th, 2024
+# Version 2.11.11: Released March 5th, 2024
+- Changes in libraries:
+  - \ref mrpt_ros1bridge_grp:
+    - Add missing mrpt::ros1bridge::toROS() for mrpt::maps::CPointsMapXYZIRT => `PointCloud2` conversions.
+- BUG FIXES:
+  - mrpt::nav::CPTG_DiffDrive_CollisionGridBased: Fix wrongly discarding of WS points out of the refDistance radius as invalid instead of returnin its closest extrapolated path pose.
+
+# Version 2.11.10: Released Feb 19th, 2024
 - Changes in libraries:
   - \ref mrpt_maps_grp:
     - mrpt::maps::CHeightGridMap2D: now supports integrating any point-cloud observation.

libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_C.h

@@ -45,7 +45,7 @@ class CPTG_DiffDrive_C : public CPTG_DiffDrive_CollisionGridBased
 	CPTG_DiffDrive_C(
 		const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
 	{
-		loadFromConfigFile(cfg, sSection);
+		CPTG_DiffDrive_C::loadFromConfigFile(cfg, sSection);
 	}
 	void loadFromConfigFile(
 		const mrpt::config::CConfigFileBase& cfg,

libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_CC.h

@@ -30,7 +30,7 @@ class CPTG_DiffDrive_CC : public CPTG_DiffDrive_CollisionGridBased
 	CPTG_DiffDrive_CC(
 		const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
 	{
-		loadFromConfigFile(cfg, sSection);
+		CPTG_DiffDrive_CC::loadFromConfigFile(cfg, sSection);
 	}
 	void loadFromConfigFile(
 		const mrpt::config::CConfigFileBase& cfg,

libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_CCS.h

@@ -30,7 +30,7 @@ class CPTG_DiffDrive_CCS : public CPTG_DiffDrive_CollisionGridBased
 	CPTG_DiffDrive_CCS(
 		const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
 	{
-		loadFromConfigFile(cfg, sSection);
+		CPTG_DiffDrive_CCS::loadFromConfigFile(cfg, sSection);
 	}
 	void loadFromConfigFile(
 		const mrpt::config::CConfigFileBase& cfg,

libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_CS.h

@@ -30,7 +30,7 @@ class CPTG_DiffDrive_CS : public CPTG_DiffDrive_CollisionGridBased
 	CPTG_DiffDrive_CS(
 		const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
 	{
-		loadFromConfigFile(cfg, sSection);
+		CPTG_DiffDrive_CS::loadFromConfigFile(cfg, sSection);
 	}
 	void loadFromConfigFile(
 		const mrpt::config::CConfigFileBase& cfg,

libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_alpha.h

@@ -40,7 +40,7 @@ class CPTG_DiffDrive_alpha : public CPTG_DiffDrive_CollisionGridBased
 	CPTG_DiffDrive_alpha(
 		const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
 	{
-		loadFromConfigFile(cfg, sSection);
+		CPTG_DiffDrive_alpha::loadFromConfigFile(cfg, sSection);
 	}
 	void loadFromConfigFile(
 		const mrpt::config::CConfigFileBase& cfg,

libs/nav/src/tpspace/CPTG_DiffDrive_CollisionGridBased.cpp

@@ -653,7 +653,7 @@ bool CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(
 	// Otherwise, it may actually mean that the target is not reachable by this
 	// set of paths:
 	const float target_dist = std::sqrt(x * x + y * y);
-	return (target_dist > target_dist);
+	return (target_dist > refDistance);
 }
 
 void CPTG_DiffDrive_CollisionGridBased::setRefDistance(const double refDist)

libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h

@@ -55,7 +55,7 @@ bool fromROS(
 bool fromROS(
 	const sensor_msgs::PointCloud2& m, mrpt::obs::CObservationRotatingScan& o,
 	const mrpt::poses::CPose3D& sensorPoseOnRobot,
-	unsigned int num_azimuth_divisions = 360);
+	unsigned int num_azimuth_divisions = 360, float max_intensity = 1000.0f);
 
 /** Extract a list of fields found in the point cloud.
  * Typically: {"x","y","z","intensity"}
@@ -75,19 +75,22 @@ bool toROS(
 	const mrpt::maps::CSimplePointsMap& obj, const std_msgs::Header& msg_header,
 	sensor_msgs::PointCloud2& msg);
 
-/** Convert mrpt::slam::CPointsMapXYZI -> sensor_msgs/PointCloud2
- * The user must supply the "msg_header" field to be copied into the output
- * message object, since that part does not appear in MRPT classes.
- *
- * Generated sensor_msgs::PointCloud2::channels: `x`, `y`, `z`, `intensity`
- *
+/** \overload With these fields: `x`, `y`, `z`, `intensity`
  * \return true on sucessful conversion, false on any error.
  * \sa fromROS
  */
 bool toROS(
 	const mrpt::maps::CPointsMapXYZI& obj, const std_msgs::Header& msg_header,
 	sensor_msgs::PointCloud2& msg);
 
+/** \overload With these fields: `x`, `y`, `z`, `intensity`, `ring`, `timestamp`
+ * \return true on successful conversion, false on any error.
+ * \sa fromROS
+ */
+bool toROS(
+	const mrpt::maps::CPointsMapXYZIRT& obj, const std_msgs::Header& msg_header,
+	sensor_msgs::PointCloud2& msg);
+
 /** @} */
 /** @} */
 

libs/ros1bridge/src/point_cloud2.cpp

@@ -147,7 +147,6 @@ bool mrpt::ros1bridge::fromROS(
 	if (incompatible || (!x_field || !y_field || !z_field || !i_field))
 		return false;
 
-	// If not, memcpy each group of contiguous fields separately
 	for (unsigned int row = 0; row < msg.height; ++row)
 	{
 		const unsigned char* row_data = &msg.data[row * msg.row_step];
@@ -162,7 +161,7 @@ bool mrpt::ros1bridge::fromROS(
 			get_float_from_field(i_field, msg_data, i);
 			obj.insertPoint(x, y, z);
 
-			obj.setPointIntensity(obj.size() - 1, i / 255.0);
+			obj.setPointIntensity(obj.size() - 1, i);
 		}
 	}
 	return true;
@@ -337,12 +336,103 @@ bool mrpt::ros1bridge::toROS(
 	return true;
 }
 
+bool mrpt::ros1bridge::toROS(
+	const CPointsMapXYZIRT& obj, const std_msgs::Header& msg_header,
+	sensor_msgs::PointCloud2& msg)
+{
+	msg.header = msg_header;
+
+	// 2D structure of the point cloud. If the cloud is unordered, height is
+	//  1 and width is the length of the point cloud.
+	msg.height = 1;
+	msg.width = obj.size();
+
+	std::vector<std::string> names = {"x", "y", "z"};
+	std::vector<size_t> offsets = {0, sizeof(float) * 1, sizeof(float) * 2};
+
+	msg.point_step = sizeof(float) * 3;
+
+	if (obj.hasIntensityField())
+	{
+		ASSERT_EQUAL_(obj.getPointsBufferRef_intensity()->size(), obj.size());
+		names.push_back("intensity");
+		offsets.push_back(msg.point_step);
+		msg.point_step += sizeof(float);
+	}
+	if (obj.hasTimeField())
+	{
+		ASSERT_EQUAL_(obj.getPointsBufferRef_timestamp()->size(), obj.size());
+		names.push_back("time");
+		offsets.push_back(msg.point_step);
+		msg.point_step += sizeof(float);
+	}
+	if (obj.hasRingField())
+	{
+		ASSERT_EQUAL_(obj.getPointsBufferRef_ring()->size(), obj.size());
+		names.push_back("ring");
+		offsets.push_back(msg.point_step);
+		msg.point_step += sizeof(uint16_t);
+	}
+
+	msg.fields.resize(names.size());
+	for (size_t i = 0; i < names.size(); i++)
+	{
+		auto& f = msg.fields.at(i);
+
+		f.count = 1;
+		f.offset = offsets[i];
+		f.datatype = (names[i] == "ring") ? sensor_msgs::PointField::UINT16
+										  : sensor_msgs::PointField::FLOAT32;
+		f.name = names[i];
+	}
+
+#if MRPT_IS_BIG_ENDIAN
+	msg.is_bigendian = true;
+#else
+	msg.is_bigendian = false;
+#endif
+
+	msg.row_step = msg.width * msg.point_step;
+
+	// data:
+	msg.data.resize(msg.row_step * msg.height);
+
+	const auto& xs = obj.getPointsBufferRef_x();
+	const auto& ys = obj.getPointsBufferRef_y();
+	const auto& zs = obj.getPointsBufferRef_z();
+	const auto& Is = *obj.getPointsBufferRef_intensity();
+	const auto& Rs = *obj.getPointsBufferRef_ring();
+	const auto& Ts = *obj.getPointsBufferRef_timestamp();
+
+	uint8_t* pointDest = msg.data.data();
+	for (size_t i = 0; i < xs.size(); i++)
+	{
+		int f = 0;
+		memcpy(pointDest + offsets[f++], &xs[i], sizeof(float));
+		memcpy(pointDest + offsets[f++], &ys[i], sizeof(float));
+		memcpy(pointDest + offsets[f++], &zs[i], sizeof(float));
+
+		if (obj.hasIntensityField())
+			memcpy(pointDest + offsets[f++], &Is[i], sizeof(float));
+
+		if (obj.hasTimeField())
+			memcpy(pointDest + offsets[f++], &Ts[i], sizeof(float));
+
+		if (obj.hasRingField())
+			memcpy(pointDest + offsets[f++], &Rs[i], sizeof(uint16_t));
+
+		pointDest += msg.point_step;
+	}
+
+	return true;
+}
+
 /** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan */
 bool mrpt::ros1bridge::fromROS(
 	const sensor_msgs::PointCloud2& msg,
 	mrpt::obs::CObservationRotatingScan& obj,
 	const mrpt::poses::CPose3D& sensorPoseOnRobot,
-	unsigned int num_azimuth_divisions)
+	unsigned int num_azimuth_divisions, float max_intensity)
 {
 	// Copy point data
 	obj.timestamp = mrpt::ros1bridge::fromROS(msg.header.stamp);
@@ -383,11 +473,31 @@ bool mrpt::ros1bridge::fromROS(
 	ASSERT_NOT_EQUAL_(ring_min, ring_max);
 
 	obj.rowCount = ring_max - ring_min + 1;
+
+	const bool inputCloudIsOrganized = msg.height != 1;
+
+	if (!num_azimuth_divisions)
+	{
+		if (inputCloudIsOrganized)
+		{
+			ASSERT_GT_(msg.width, 0U);
+			num_azimuth_divisions = msg.width;
+		}
+		else
+		{
+			THROW_EXCEPTION(
+				"An explicit value for num_azimuth_divisions must be given if "
+				"the input cloud is not 'organized'");
+		}
+	}
+
 	obj.columnCount = num_azimuth_divisions;
 
 	obj.rangeImage.resize(obj.rowCount, obj.columnCount);
 	obj.rangeImage.fill(0);
 
+	obj.sensorPose = sensorPoseOnRobot;
+
 	// Default unit: 1cm
 	if (obj.rangeResolution == 0) obj.rangeResolution = 1e-2;
 
@@ -399,6 +509,11 @@ bool mrpt::ros1bridge::fromROS(
 	else
 		obj.intensityImage.resize(0, 0);
 
+	if (inputCloudIsOrganized)
+	{
+		obj.organizedPoints.resize(obj.rowCount, obj.columnCount);
+	}
+
 	// If not, memcpy each group of contiguous fields separately
 	for (unsigned int row = 0; row < msg.height; ++row)
 	{
@@ -414,15 +529,24 @@ bool mrpt::ros1bridge::fromROS(
 			get_float_from_field(z_field, msg_data, z);
 			get_uint16_from_field(ring_field, msg_data, ring_id);
 
-			const mrpt::math::TPoint3D localPt =
-				sensorPoseOnRobot.inverseComposePoint(
-					mrpt::math::TPoint3D(x, y, z));
-			const double azimuth = std::atan2(localPt.y, localPt.x);
+			const mrpt::math::TPoint3D localPt = {x, y, z};
 
-			const auto az_idx = lround(
-				(num_azimuth_divisions - 1) * (azimuth + M_PI) / (2 * M_PI));
-			ASSERT_GE_(az_idx, 0);
-			ASSERT_LE_(az_idx, num_azimuth_divisions - 1);
+			unsigned int az_idx;
+			if (inputCloudIsOrganized)
+			{
+				// "azimuth index" is just the "column":
+				az_idx = col;
+			}
+			else
+			{
+				// Recover "azimuth index" from trigonometry:
+				const double azimuth = std::atan2(localPt.y, localPt.x);
+
+				az_idx = lround(
+					(num_azimuth_divisions - 1) * (azimuth + M_PI) /
+					(2 * M_PI));
+				ASSERT_LE_(az_idx, num_azimuth_divisions - 1);
+			}
 
 			// Store in matrix form:
 			obj.rangeImage(ring_id, az_idx) =
@@ -432,10 +556,14 @@ bool mrpt::ros1bridge::fromROS(
 			{
 				float intensity;
 				get_float_from_field(i_field, msg_data, intensity);
-				ASSERT_LE_(intensity, 255.0f);
-				obj.intensityImage(ring_id, az_idx) = lround(intensity);
+				obj.intensityImage(ring_id, az_idx) =
+					lround(255 * intensity / max_intensity);
 			}
+
+			if (inputCloudIsOrganized)
+				obj.organizedPoints(ring_id, az_idx) = localPt;
 		}
 	}
+
 	return true;
 }

libs/ros1bridge/src/pointcloud2_unittest.cpp

@@ -62,9 +62,7 @@ TEST(PointCloud2, basicTest)
 
 		i_f += 1.0;
 	}
-	//;
 }
-
 #endif	// HAVE_PCL
 
 TEST(PointCloud2, toROS)

libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h

@@ -94,7 +94,7 @@ bool toROS(
 	const std_msgs::msg::Header& msg_header,
 	sensor_msgs::msg::PointCloud2& msg);
 
-/** \overload With these fields: `x`, `y`, `z`, `intensity`
+/** \overload With these fields: `x`, `y`, `z`, `intensity`, `ring`, `timestamp`
  * \return true on successful conversion, false on any error.
  * \sa fromROS
  */

libs/ros2bridge/src/point_cloud2.cpp

@@ -504,8 +504,8 @@ bool mrpt::ros2bridge::fromROS(
 
 	obj.sensorPose = sensorPoseOnRobot;
 
-	// Default unit: 5mm
-	if (obj.rangeResolution == 0) obj.rangeResolution = 5e-3;
+	// Default unit: 1cm
+	if (obj.rangeResolution == 0) obj.rangeResolution = 1e-2;
 
 	if (i_field)
 	{

package.xml

@@ -7,7 +7,7 @@
 <package format="3">
   <name>mrpt2</name>
   <!-- Before updating version number, read [MRPT_ROOT]/version_prefix.txt first -->
-  <version>2.11.10</version>
+  <version>2.11.11</version>
   <description>Mobile Robot Programming Toolkit (MRPT) version 2.x</description>
 
   <author     email="[email protected]">Jose-Luis Blanco-Claraco</author>

version_prefix.txt

@@ -1,4 +1,4 @@
-2.11.10
+2.11.11
 # IMPORTANT: This file is parsed by CMake, don't add any comment to
 # the first line.
 # This file is used in both Windows and Linux scripts to automatically