Merge branch 'develop'

README.md

@@ -78,7 +78,7 @@ sudo apt install ros-$ROS_DISTRO-mrpt2
 | ROS1 Noetic @ u20.04 | [![Build Status](https://build.ros.org/job/Ndev__mrpt2__ubuntu_focal_amd64/badge/icon)](https://build.ros.org/job/Ndev__mrpt2__ubuntu_focal_amd64/) | [![Version](https://img.shields.io/ros/v/noetic/mrpt2)](https://index.ros.org/search/?term=mrpt2) | [![Build Status](https://build.ros.org/job/Nbin_uF64__mrpt2__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros.org/job/Nbin_uF64__mrpt2__ubuntu_focal_amd64__binary/) |
 | ROS2 Humble @ u22.04 | [![Build Status](https://build.ros2.org/job/Hdev__mrpt2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__mrpt2__ubuntu_jammy_amd64/) | [![Version](https://img.shields.io/ros/v/humble/mrpt2)](https://index.ros.org/search/?term=mrpt2) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mrpt2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mrpt2__ubuntu_jammy_amd64__binary/) | 
 | ROS2 Iron @ u22.04 | [![Build Status](https://build.ros2.org/job/Idev__mrpt2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Idev__mrpt2__ubuntu_jammy_amd64/) | [![Version](https://img.shields.io/ros/v/iron/mrpt2)](https://index.ros.org/search/?term=mrpt2) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mrpt2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mrpt2__ubuntu_jammy_amd64__binary/) | 
-| ROS2 Rolling @ u22.04 | [![Build Status](https://build.ros2.org/job/Rdev__mrpt2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Rdev__mrpt2__ubuntu_jammy_amd64/) |  [![Version](https://img.shields.io/ros/v/rolling/mrpt2)](https://index.ros.org/search/?term=mrpt2) | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__mrpt2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__mrpt2__ubuntu_jammy_amd64__binary/) | 
+| ROS2 Rolling @ u24.04 | [![Build Status](https://build.ros2.org/job/Rdev__mrpt2__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Rdev__mrpt2__ubuntu_noble_amd64/) |  [![Version](https://img.shields.io/ros/v/rolling/mrpt2)](https://index.ros.org/search/?term=mrpt2) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mrpt2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mrpt2__ubuntu_noble_amd64__binary/) | 
 
 | EOL Distro | Last release |
 |---|---|

appveyor.yml

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

doc/source/doxygen-docs/changelog.md

@@ -1,5 +1,17 @@
 \page changelog Change Log
 
+# Version 2.11.12: Released March 10th, 2024
+- Changes in libraries:
+  - \ref mrpt_obs_grp:
+    - Fix compiler error on invocation of template mrpt::obs::CObservationGPS::getMsgByClassPtr()
+    - Add field mrpt::obs::CObservationGPS::covariance_enu for easier interoperability with ROS.
+    - API simplified: replace custom `mrpt::obs::gnss::gnss_message_ptr` with a `std::shared_ptr<>`.
+    - mrpt::obs::CObservationRobotPose::getDescriptionAsText(): add human-readable pose uncertainties.
+- BUG FIXES:
+  - Fix wrong encoding of grayscale images in mrpt::ros1bridge::toROS() and mrpt::ros2bridge::toROS().
+  - Correctly return true/false in conversion of GPS observation in mrpt::ros1bridge::toROS() and mrpt::ros2bridge::toROS() depending on whether there is a valid GGA message.
+  - mrpt::obs::CObservationComment: Missing serialization of sensorLabel.
+
 # Version 2.11.11: Released March 5th, 2024
 - Changes in libraries:
   - \ref mrpt_ros1bridge_grp:

libs/hwdrivers/include/mrpt/hwdrivers/CGPSInterface.h

@@ -355,8 +355,11 @@ class CGPSInterface : public mrpt::system::COutputLogger, public CGenericSensor
 	/** Queue out now the messages in \a m_just_parsed_messages, leaving it
 	 * empty */
 	void flushParsedMessagesNow();
+
 	/** A private copy of the last received gps datum */
-	mrpt::obs::CObservationGPS m_just_parsed_messages;
+	mrpt::obs::CObservationGPS::Ptr m_parsed_messages =
+		mrpt::obs::CObservationGPS::Create();
+
 	/** Used in getLastGGA() */
 	std::string m_last_GGA;
 };	// end class

libs/hwdrivers/src/CGPSInterface.cpp

@@ -375,15 +375,14 @@ void CGPSInterface::doProcess()
 		{
 			if (m_verbose)
 				cout << "[CGPSInterface] Initial timestamp: "
-					 << mrpt::system::timeToString(
-							m_just_parsed_messages.timestamp)
+					 << mrpt::system::timeToString(m_parsed_messages->timestamp)
 					 << endl;
 			// Check if the initial timestamp seems to be OK (not a spurio one)
 			TTimeStamp tmNow = mrpt::system::now();
 			const double tdif = mrpt::system::timeDifference(
-				m_just_parsed_messages.timestamp, tmNow);
+				m_parsed_messages->timestamp, tmNow);
 			if (tdif >= 0 && tdif < 7500 /*Up to two hours*/)
-				m_last_timestamp = m_just_parsed_messages.timestamp;
+				m_last_timestamp = m_parsed_messages->timestamp;
 			else
 			{
 				if (m_verbose)
@@ -395,7 +394,7 @@ void CGPSInterface::doProcess()
 		else
 		{
 			const double time_diff = mrpt::system::timeDifference(
-				m_last_timestamp, m_just_parsed_messages.timestamp);
+				m_last_timestamp, m_parsed_messages->timestamp);
 			if (time_diff < 0 || time_diff > 300)  // Assert that the current
 			// timestamp is after the
 			// previous one and not more
@@ -418,8 +417,8 @@ void CGPSInterface::doProcess()
 			// a. These GPS data have both synched RMC and GGA data
 			// don't append observation until we have both data
 			do_append_obs =
-				(m_just_parsed_messages.has_GGA_datum() &&
-				 m_just_parsed_messages.has_RMC_datum());
+				(m_parsed_messages->has_GGA_datum() &&
+				 m_parsed_messages->has_RMC_datum());
 		}  // end-else
 
 		if (do_append_obs) flushParsedMessagesNow();
@@ -431,18 +430,19 @@ void CGPSInterface::doProcess()
 void CGPSInterface::flushParsedMessagesNow()
 {
 	// Generic observation data:
-	m_just_parsed_messages.sensorPose = m_sensorPose;
+	m_parsed_messages->sensorPose = m_sensorPose;
 	if (m_sensorLabelAppendMsgType)
-		m_just_parsed_messages.sensorLabel =
-			m_sensorLabel + string("_") + m_just_parsed_messages.sensorLabel;
+		m_parsed_messages->sensorLabel =
+			m_sensorLabel + string("_") + m_parsed_messages->sensorLabel;
 	else
-		m_just_parsed_messages.sensorLabel = m_sensorLabel;
+		m_parsed_messages->sensorLabel = m_sensorLabel;
+
+	m_last_timestamp = m_parsed_messages->timestamp;
+
 	// Add observation to the output queue:
-	CObservationGPS::Ptr newObs = std::make_shared<CObservationGPS>();
-	m_just_parsed_messages.swap(*newObs);
-	CGenericSensor::appendObservation(newObs);
-	m_just_parsed_messages.clear();
-	m_last_timestamp = m_just_parsed_messages.timestamp;
+	CGenericSensor::appendObservation(m_parsed_messages);
+
+	m_parsed_messages = mrpt::obs::CObservationGPS::Create();
 
 	// And this means the comms works:
 	m_GPS_comsWork = true;
@@ -481,7 +481,7 @@ void CGPSInterface::parseBuffer()
 					m_rx_buffer.pop();	// Not the start of a frame, skip 1 byte
 			}
 			if (m_customInit.empty() /* If we are not in old legacy mode */ &&
-				!m_just_parsed_messages.messages.empty())
+				!m_parsed_messages->messages.empty())
 				flushParsedMessagesNow();
 		} while (m_rx_buffer.size() >= min_bytes);
 	}  // end one parser mode ----------
@@ -508,8 +508,9 @@ void CGPSInterface::parseBuffer()
 				m_rx_buffer.pop();	// Not the start of a frame, skip 1 byte
 
 			if (m_customInit.empty() /* If we are not in old legacy mode */ &&
-				!m_just_parsed_messages.messages.empty())
+				!m_parsed_messages->messages.empty())
 				flushParsedMessagesNow();
+
 		} while (m_rx_buffer.size() >= global_min_bytes_max);
 	}  // end AUTO mode ----
 }
@@ -605,7 +606,7 @@ bool CGPSInterface::OnConnectionShutdown()
 bool CGPSInterface::OnConnectionEstablished()
 {
 	m_last_GGA.clear();	 // On comms reset, empty this cache
-	m_just_parsed_messages.clear();
+	m_parsed_messages->clear();
 
 	// Legacy behavior:
 	if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||

libs/hwdrivers/src/CGPSInterface_parser_NMEA.cpp

@@ -74,15 +74,15 @@ bool CGPSInterface::implement_parser_NMEA(size_t& out_minimum_rx_buf_to_decide)
 			m_rx_buffer.pop();
 
 		// Parse:
-		const bool did_have_gga = m_just_parsed_messages.has_GGA_datum();
+		const bool did_have_gga = m_parsed_messages->has_GGA_datum();
 		if (CGPSInterface::parse_NMEA(
-				line, m_just_parsed_messages, false /*verbose*/))
+				line, *m_parsed_messages, false /*verbose*/))
 		{
 			// Parsers must set only the part of the msg type:
-			m_just_parsed_messages.sensorLabel = "NMEA";
+			m_parsed_messages->sensorLabel = "NMEA";
 
 			// Save GGA cache (useful for NTRIP,...)
-			const bool now_has_gga = m_just_parsed_messages.has_GGA_datum();
+			const bool now_has_gga = m_parsed_messages->has_GGA_datum();
 			if (now_has_gga && !did_have_gga) { m_last_GGA = line; }
 		}
 		else

libs/hwdrivers/src/CGPSInterface_parser_NOVATEL_OEM6.cpp

@@ -119,16 +119,16 @@ bool CGPSInterface::implement_parser_NOVATEL_OEM6(
 					  << hdr.msg_id << "\n";
 			return true;
 		}
-		m_just_parsed_messages.messages[msg->message_type] = msg;
-		m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
+		m_parsed_messages->messages[msg->message_type] = msg;
+		m_parsed_messages->originalReceivedTimestamp = mrpt::system::now();
 		if (!CObservationGPS::GPS_time_to_UTC(
 				hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
-				m_just_parsed_messages.timestamp))
-			m_just_parsed_messages.timestamp = mrpt::system::now();
+				m_parsed_messages->timestamp))
+			m_parsed_messages->timestamp = mrpt::system::now();
 		else
-			m_just_parsed_messages.has_satellite_timestamp = true;
+			m_parsed_messages->has_satellite_timestamp = true;
 
-		m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
+		m_parsed_messages->sensorLabel = msg->getMessageTypeAsString();
 
 		flushParsedMessagesNow();
 		return true;
@@ -197,8 +197,8 @@ bool CGPSInterface::implement_parser_NOVATEL_OEM6(
 					  << hdr.msg_id << "\n";
 			return true;
 		}
-		m_just_parsed_messages.messages[msg->message_type] = msg;
-		m_just_parsed_messages.originalReceivedTimestamp = mrpt::system::now();
+		m_parsed_messages->messages[msg->message_type] = msg;
+		m_parsed_messages->originalReceivedTimestamp = mrpt::system::now();
 		{
 			// Detect NV_OEM6_IONUTC msgs to learn about the current leap
 			// seconds:
@@ -208,12 +208,12 @@ bool CGPSInterface::implement_parser_NOVATEL_OEM6(
 		}
 		if (!CObservationGPS::GPS_time_to_UTC(
 				hdr.week, hdr.ms_in_week * 1e-3, num_leap_seconds,
-				m_just_parsed_messages.timestamp))
-			m_just_parsed_messages.timestamp = mrpt::system::now();
+				m_parsed_messages->timestamp))
+			m_parsed_messages->timestamp = mrpt::system::now();
 		else
-			m_just_parsed_messages.has_satellite_timestamp = true;
+			m_parsed_messages->has_satellite_timestamp = true;
 
-		m_just_parsed_messages.sensorLabel = msg->getMessageTypeAsString();
+		m_parsed_messages->sensorLabel = msg->getMessageTypeAsString();
 		flushParsedMessagesNow();
 		return true;
 	}  // end regular hdr

libs/hwdrivers/src/CGPSInterface_unittest.cpp

@@ -370,19 +370,19 @@ TEST(CGPSInterface, parse_NMEA_stream)
 	EXPECT_TRUE(obsGPS2);
 	EXPECT_TRUE(obsGPS3);
 
-	const auto* msg1 = obsGPS1->getMsgByClassPtr<gnss::Message_NMEA_GSA>();
-	EXPECT_TRUE(msg1 != nullptr);
-	if (!msg1) return;
-	EXPECT_EQ(msg1->fields.PRNs[0][0], '1');
-	EXPECT_EQ(msg1->fields.PRNs[0][1], '5');
-	EXPECT_NEAR(msg1->fields.HDOP, 2.31, 0.1);
-
-	const auto* msg2 = obsGPS2->getMsgByClassPtr<gnss::Message_NMEA_GSA>();
-	EXPECT_TRUE(msg2 != nullptr);
-
-	const auto* msg3 = obsGPS3->getMsgByClassPtr<gnss::Message_NMEA_RMC>();
-	EXPECT_TRUE(msg3 != nullptr);
-	if (!msg3) return;
-	EXPECT_NEAR(msg3->fields.longitude_degrees, -2.407810500, 0.0001);
-	EXPECT_NEAR(msg3->fields.latitude_degrees, 36.829821500, 0.0001);
+	const auto* msgRMC = obsGPS1->getMsgByClassPtr<gnss::Message_NMEA_RMC>();
+	EXPECT_TRUE(msgRMC != nullptr);
+	if (!msgRMC) return;
+	EXPECT_NEAR(msgRMC->fields.longitude_degrees, -2.407810500, 0.0001);
+	EXPECT_NEAR(msgRMC->fields.latitude_degrees, 36.829821500, 0.0001);
+
+	const auto* msgGSA1 = obsGPS2->getMsgByClassPtr<gnss::Message_NMEA_GSA>();
+	EXPECT_TRUE(msgGSA1 != nullptr);
+	if (!msgGSA1) return;
+	EXPECT_EQ(msgGSA1->fields.PRNs[0][0], '1');
+	EXPECT_EQ(msgGSA1->fields.PRNs[0][1], '5');
+	EXPECT_NEAR(msgGSA1->fields.HDOP, 2.31, 0.1);
+
+	const auto* msgGSA2 = obsGPS3->getMsgByClassPtr<gnss::Message_NMEA_GSA>();
+	EXPECT_TRUE(msgGSA2 != nullptr);
 }

libs/obs/include/mrpt/obs/CObservationGPS.h

@@ -8,13 +8,15 @@
    +------------------------------------------------------------------------+ */
 #pragma once
 
+#include <mrpt/math/CMatrixFixed.h>
 #include <mrpt/obs/CObservation.h>
 #include <mrpt/obs/gnss_messages.h>
 #include <mrpt/poses/CPose2D.h>
 #include <mrpt/poses/CPose3D.h>
 #include <mrpt/serialization/CSerializable.h>
 
 #include <map>
+#include <optional>
 #include <typeinfo>
 
 namespace mrpt::obs
@@ -28,7 +30,7 @@ namespace mrpt::obs
  *  indivual messages \b stored in mrpt::obs::CObservationGPS objects.
  *
  *  Supported message types are:
- *  - NMEA 0183 (ASCII): GGA, RMC
+ *  - NMEA 0183 (ASCII): GGA, RMC, etc.
  *  - Topcon GRIL (Binary): PZS, SATS
  *  - Novatel GNSS/SPAN OEM6 (Binary): See list of log packets under namespace
  * mrpt::obs::gnss and in enum type mrpt::obs::gnss::gnss_message_type_t
@@ -57,11 +59,9 @@ namespace mrpt::obs
  * }
  * \endcode
  *
- * \note <b>[API changed in MRPT 1.4.0]</b> mrpt::obs::CObservationGPS now
- * stores message objects in a more flexible way. API clean-up and extended so
- * the number of GNSS message types is larger and more scalable.
- * \note Porting old code: For example, replace `observation.GGA_datum.XXX` with
- * `observation.getMsgByClass<gnss::Message_NMEA_GGA>().fields.XXX`, etc.
+ * \note Since MRPT 2.11.12 there is an optional field for ENU covariance for
+ * easier compatibility with ROS messages.
+ *
  * \sa CObservation
  * \ingroup mrpt_obs_grp
  */
@@ -77,20 +77,27 @@ class CObservationGPS : public CObservation
 	 * @{ */
 	/** The sensor pose on the robot/vehicle */
 	mrpt::poses::CPose3D sensorPose{};
+
 	/** The local computer-based timestamp based on the reception of the message
 	 * in the computer. \sa CObservation::timestamp in the base class, which
 	 * should contain the accurate satellite-based UTC timestamp. */
 	mrpt::system::TTimeStamp originalReceivedTimestamp{INVALID_TIMESTAMP};
+
 	/** If true, CObservation::timestamp has been generated from accurate
 	 * satellite clock. Otherwise, no GPS data is available and timestamps are
 	 * based on the local computer clock. */
 	bool has_satellite_timestamp{false};
+
 	/** The main piece of data in this class: a list of GNNS messages.
 	 * Normally users might prefer to access the list via the methods
 	 * CObservationGPS::getMsgByClass() and CObservationGPS::setMsg()
 	 * Typically only one message, may be multiple if all have the same
 	 * timestamp. */
 	message_list_t messages;
+
+	/** If present, it defines the ENU position uncertainty.
+	 */
+	std::optional<mrpt::math::CMatrixDouble33> covariance_enu;
 	/** @} */
 
 	/** @name Main API to access to the data fields
@@ -104,7 +111,7 @@ class CObservationGPS : public CObservation
 	void setMsg(const MSG_CLASS& msg)
 	{
 		messages[static_cast<gnss::gnss_message_type_t>(MSG_CLASS::msg_type)]
-			.set(new MSG_CLASS(msg));
+			.reset(new MSG_CLASS(msg));
 	}
 	/** Returns true if the list \a CObservationGPS::messages contains one of
 	 * the requested type. \sa mrpt::obs::gnss::gnss_message_type_t,
@@ -189,8 +196,8 @@ class CObservationGPS : public CObservation
 		auto it = messages.find(
 			static_cast<gnss::gnss_message_type_t>(MSG_CLASS::msg_type));
 		return it == messages.end()
-			? dynamic_cast<MSG_CLASS*>(nullptr)
-			: dynamic_cast<MSG_CLASS*>(it->second.get());
+			? static_cast<const MSG_CLASS*>(nullptr)
+			: dynamic_cast<const MSG_CLASS*>(it->second.get());
 	}
 
 	/** Dumps the contents of the observation in a human-readable form to a
@@ -201,7 +208,6 @@ class CObservationGPS : public CObservation
 	void dumpToConsole(std::ostream& o) const;
 	/** Empties this observation, clearing the container \a messages */
 	void clear();
-	void swap(CObservationGPS& o);
 
 	void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
 	{

libs/obs/include/mrpt/obs/gnss_messages_common.h

@@ -97,42 +97,7 @@ struct gnss_message
 
 /** A smart pointer to a GNSS message. \sa gnss_message,
  * mrpt::obs::CObservationGPS  */
-struct gnss_message_ptr
-{
-   protected:
-	gnss_message* ptr{nullptr};
-
-   public:
-	/** Ctor (default: nullptr pointer) */
-	gnss_message_ptr();
-	/** Makes a copy of the pointee */
-	gnss_message_ptr(const gnss_message_ptr& o);
-	/** Assigns a pointer. Memory now belongs to this class. */
-	explicit gnss_message_ptr(const gnss_message* p);
-	gnss_message_ptr& operator=(
-		const gnss_message_ptr& o);	 // Makes a copy of the pointee
-	/** Dtor: it frees the pointee memory */
-	virtual ~gnss_message_ptr();
-	bool operator==(const gnss_message* o) const { return o == ptr; }
-	bool operator==(const gnss_message_ptr& o) const { return o.ptr == ptr; }
-	bool operator!=(const gnss_message* o) const { return o != ptr; }
-	bool operator!=(const gnss_message_ptr& o) const { return o.ptr != ptr; }
-	gnss_message*& get() { return ptr; }
-	const gnss_message* get() const { return ptr; }
-	gnss_message*& operator->()
-	{
-		ASSERT_(ptr);
-		return ptr;
-	}
-	const gnss_message* operator->() const
-	{
-		ASSERT_(ptr);
-		return ptr;
-	}
-	/** Replaces the pointee with a new pointer. Its memory now belongs to this
-	 * object, do not free manually. */
-	void set(gnss_message* p);
-};
+using gnss_message_ptr = std::shared_ptr<gnss_message>;
 
 #define GNSS_MESSAGE_BINARY_BLOCK(DATA_PTR, DATA_LEN)                          \
    protected:                                                                  \