Address new field variance in ROS msg Range (Closes #1270)

doc/source/doxygen-docs/changelog.md

@@ -4,6 +4,10 @@
 - Changes in libraries:
   - \ref mrpt_gui_grp
     - New function mrpt::gui::GetScaledClientSize().
+  - \ref mrpt_obs_grp
+    - Fix typo in data field: mrpt::obs::CObservationRange: `sensorConeApperture`  -> `sensorConeAperture`
+  - \ref mrpt_ros2bridge_grp
+    - Address the new field `variance` in `sensor_msgs/Range` (Closes [issue #1270](https://github.com/MRPT/mrpt/issues/1270)).
 - BUG FIXES:
   - Fix wrong rendering of all wxWidgets-based OpenGL windows when using Ubuntu's display settings to change UI to a size different than 100% (Fixes [issue #1114](https://github.com/MRPT/mrpt/issues/1114)).
 

libs/hwdrivers/src/CBoardSonars.cpp

@@ -214,7 +214,7 @@ bool CBoardSonars::getObservation(mrpt::obs::CObservationRange& obs)
 		obs.timestamp = mrpt::system::getCurrentTime();
 		obs.minSensorDistance = 0.04f;
 		obs.maxSensorDistance = m_maxRange;
-		obs.sensorConeApperture = DEG2RAD(30.0f);
+		obs.sensorConeAperture = DEG2RAD(30.0f);
 		obs.sensedData.clear();
 		mrpt::obs::CObservationRange::TMeasurement obsRange;
 

libs/hwdrivers/src/CPhidgetInterfaceKitProximitySensors.cpp

@@ -268,7 +268,7 @@ void CPhidgetInterfaceKitProximitySensors::getObservation(
 	obs.sensorLabel = m_sensorLabel;
 	obs.minSensorDistance = m_minOfMinRanges;
 	obs.maxSensorDistance = m_maxOfMaxRanges;
-	obs.sensorConeApperture =
+	obs.sensorConeAperture =
 		DEG2RAD(2.0f);	// TODO : Adapt to real sensor cone apperture.
 	obs.sensedData.clear();
 

libs/maps/src/maps/COccupancyGridMap2D_insert.cpp

@@ -937,7 +937,7 @@ bool COccupancyGridMap2D::internal_insertObservation(
 
 			// Now go and insert the triangles of each beam:
 			// -----------------------------------------------
-			A = d2f(laserPose.phi()) - 0.5f * o.sensorConeApperture;
+			A = d2f(laserPose.phi()) - 0.5f * o.sensorConeAperture;
 			dAK = 0;
 
 			// Insert the rays:
@@ -947,7 +947,7 @@ bool COccupancyGridMap2D::internal_insertObservation(
 
 			last_valid_range = maxDistanceInsertion;
 
-			const float dA_2 = 0.5f * o.sensorConeApperture;
+			const float dA_2 = 0.5f * o.sensorConeAperture;
 			for (idx = 0; idx < nRanges; idx += K, A += dAK)
 			{
 				float theR;	 // The range of this beam

libs/maps/src/maps/COccupancyGridMap2D_simulate.cpp

@@ -82,13 +82,13 @@ void COccupancyGridMap2D::sonarSimulator(
 
 		// For each sonar cone, simulate several rays and keep the shortest
 		// distance:
-		ASSERT_(inout_observation.sensorConeApperture > 0);
+		ASSERT_(inout_observation.sensorConeAperture > 0);
 		size_t nRays =
-			round(1 + inout_observation.sensorConeApperture / 1.0_deg);
+			round(1 + inout_observation.sensorConeAperture / 1.0_deg);
 
 		double direction = sensorAbsolutePose.phi() -
-			0.5 * inout_observation.sensorConeApperture;
-		const double Adir = inout_observation.sensorConeApperture / nRays;
+			0.5 * inout_observation.sensorConeAperture;
+		const double Adir = inout_observation.sensorConeAperture / nRays;
 
 		float min_detected_obs = 0;
 		for (size_t i = 0; i < nRays; i++, direction += Adir)

libs/maps/src/maps/CPointsMap.cpp

@@ -1847,7 +1847,7 @@ bool CPointsMap::internal_insertObservation(
 
 		const auto& o = static_cast<const CObservationRange&>(obs);
 
-		const double aper_2 = 0.5 * o.sensorConeApperture;
+		const double aper_2 = 0.5 * o.sensorConeAperture;
 
 		this->reserve(
 			this->size() + o.sensedData.size() * 30);  // faster push_back's.
@@ -1863,9 +1863,9 @@ bool CPointsMap::internal_insertObservation(
 
 			// Insert a few points with a given maximum separation between
 			// them:
-			const double arc_len = o.sensorConeApperture * rang;
+			const double arc_len = o.sensorConeAperture * rang;
 			const unsigned int nSteps = round(1 + arc_len / 0.05);
-			const double Aa = o.sensorConeApperture / double(nSteps);
+			const double Aa = o.sensorConeAperture / double(nSteps);
 			TPoint3D loc, glob;
 
 			for (double a1 = -aper_2; a1 < aper_2; a1 += Aa)

libs/obs/include/mrpt/obs/CObservationRange.h

@@ -37,15 +37,16 @@ class CObservationRange : public CObservation
 	float maxSensorDistance{5};
 
 	/** Cone aperture of each ultrasonic beam, in radians. */
-	float sensorConeApperture = mrpt::d2f(20.0_deg);
+	float sensorConeAperture = mrpt::d2f(20.0_deg);
 
 	struct TMeasurement
 	{
-		TMeasurement() : sensorPose() {}
+		TMeasurement() = default;
+
 		/** Some kind of sensor ID which identifies it on the bus (if
 		 * applicable, 0 otherwise)
 		 */
-		uint16_t sensorID{0};
+		uint16_t sensorID = 0;
 
 		/** The 6D position of the sensor on the robot.
 		 */
@@ -54,7 +55,10 @@ class CObservationRange : public CObservation
 		/** The measured range, in meters (or a value of 0 if there was no
 		 * detected echo).
 		 */
-		float sensedDistance{0};
+		float sensedDistance = 0;
+
+		/** If !=0, the nominal sensor noise, as one standard deviation */
+		float sensorNoiseStdDeviation = 0;
 	};
 
 	using TMeasurementList = std::deque<TMeasurement>;

libs/obs/src/CObservationRange.cpp

@@ -18,16 +18,17 @@ using namespace mrpt::poses;
 // This must be added to any CSerializable class implementation file.
 IMPLEMENTS_SERIALIZABLE(CObservationRange, CObservation, mrpt::obs)
 
-uint8_t CObservationRange::serializeGetVersion() const { return 3; }
+uint8_t CObservationRange::serializeGetVersion() const { return 4; }
 void CObservationRange::serializeTo(mrpt::serialization::CArchive& out) const
 {
 	// The data
-	out << minSensorDistance << maxSensorDistance << sensorConeApperture;
+	out << minSensorDistance << maxSensorDistance << sensorConeAperture;
 	const uint32_t n = sensedData.size();
 	out << n;
 	for (uint32_t i = 0; i < n; i++)
 		out << sensedData[i].sensorID << CPose3D(sensedData[i].sensorPose)
-			<< sensedData[i].sensedDistance;
+			<< sensedData[i].sensedDistance
+			<< sensedData[i].sensorNoiseStdDeviation;  // v4
 	out << sensorLabel << timestamp;
 }
 
@@ -40,13 +41,15 @@ void CObservationRange::serializeFrom(
 		case 1:
 		case 2:
 		case 3:
+		case 4:
 		{
 			uint32_t i, n;
 
 			// The data
-			in >> minSensorDistance >> maxSensorDistance >> sensorConeApperture;
+			in >> minSensorDistance >> maxSensorDistance >> sensorConeAperture;
 
 			in >> n;
+			sensedData.clear();
 			sensedData.resize(n);
 			CPose3D aux;
 			for (i = 0; i < n; i++)
@@ -57,6 +60,8 @@ void CObservationRange::serializeFrom(
 
 				in >> aux >> sensedData[i].sensedDistance;
 				sensedData[i].sensorPose = aux.asTPose();
+
+				if (version >= 4) in >> sensedData[i].sensorNoiseStdDeviation;
 			}
 
 			if (version >= 1) in >> sensorLabel;
@@ -96,18 +101,19 @@ void CObservationRange::getDescriptionAsText(std::ostream& o) const
 	using namespace std;
 	CObservation::getDescriptionAsText(o);
 
-	o << "minSensorDistance   = " << minSensorDistance << " m" << endl;
-	o << "maxSensorDistance   = " << maxSensorDistance << " m" << endl;
-	o << "sensorConeApperture = " << RAD2DEG(sensorConeApperture) << " deg"
-	  << endl;
+	o << "minSensorDistance       = " << minSensorDistance << " m\n";
+	o << "maxSensorDistance       = " << maxSensorDistance << " m\n";
+	o << "sensorConeAperture     = " << RAD2DEG(sensorConeAperture) << " deg\n";
 
 	// For each entry in this sequence:
-	o << "  SENSOR_ID    RANGE (m)    SENSOR POSE (on the robot)" << endl;
-	o << "-------------------------------------------------------" << endl;
+	o << "  SENSOR_ID    RANGE (m)   STD_DEV (m)  SENSOR POSE (on the robot) "
+		 "\n";
+	o << "-------------------------------------------------------\n";
 	for (const auto& q : sensedData)
 	{
 		o << format("     %7u", (unsigned int)q.sensorID);
 		o << format("    %4.03f   ", q.sensedDistance);
-		o << q.sensorPose << endl;
+		o << format("    %4.03f   ", q.sensorNoiseStdDeviation);
+		o << q.sensorPose << "\n";
 	}
 }

libs/ros1bridge/src/range.cpp

@@ -20,10 +20,9 @@ bool mrpt::ros1bridge::fromROS(
 {
 	obj.minSensorDistance = msg.min_range;
 	obj.maxSensorDistance = msg.max_range;
-	obj.sensorConeApperture = msg.field_of_view;
+	obj.sensorConeAperture = msg.field_of_view;
 
-	/// again this is amibiguous as can't be certain of number of measurement
-	/// from corresponding ROS message
+	obj.sensedData.resize(1);
 	obj.sensedData.at(0).sensedDistance = msg.range;
 	return true;
 }
@@ -43,7 +42,7 @@ bool mrpt::ros1bridge::toROS(
 	{
 		msg[i].max_range = obj.maxSensorDistance;
 		msg[i].min_range = obj.minSensorDistance;
-		msg[i].field_of_view = obj.sensorConeApperture;
+		msg[i].field_of_view = obj.sensorConeAperture;
 	}
 
 	/// following part needs to be double checked, it looks incorrect

libs/ros2bridge/src/range.cpp

@@ -15,41 +15,66 @@
 
 #include <mrpt/ros2bridge/range.h>
 
+#include <cmath>
+
+template <class T, class = void>
+struct has_variance : std::false_type
+{
+};
+
+template <class T>
+struct has_variance<T, std::void_t<decltype(T::variance)>> : std::true_type
+{
+};
+
 bool mrpt::ros2bridge::fromROS(
 	const sensor_msgs::msg::Range& msg, mrpt::obs::CObservationRange& obj)
 {
 	obj.minSensorDistance = msg.min_range;
 	obj.maxSensorDistance = msg.max_range;
-	obj.sensorConeApperture = msg.field_of_view;
+	obj.sensorConeAperture = msg.field_of_view;
 
-	/// again this is amibiguous as can't be certain of number of measurement
-	/// from corresponding ROS message
+	obj.sensedData.resize(1);
 	obj.sensedData.at(0).sensedDistance = msg.range;
+
+	// See: https://github.com/MRPT/mrpt/issues/1270
+	if constexpr (has_variance<sensor_msgs::msg::Range>::value)
+	{
+		obj.sensedData.at(0).sensorNoiseStdDeviation = std::sqrt(msg.variance);
+	}
+
 	return true;
 }
 
 bool mrpt::ros2bridge::toROS(
 	const mrpt::obs::CObservationRange& obj,
 	const std_msgs::msg::Header& msg_header, sensor_msgs::msg::Range* msg)
 {
-	long num_range = obj.sensedData.size();
+	const auto num_range = obj.sensedData.size();
 
 	// 1) sensor_msgs::msg::Range:: header
-	for (long i = 0; i < num_range; i++)
+	for (size_t i = 0; i < num_range; i++)
 		msg[i].header = msg_header;
 
 	// 2) sensor_msg::Range parameters
-	for (long i = 0; i < num_range; i++)
+	for (size_t i = 0; i < num_range; i++)
 	{
 		msg[i].max_range = obj.maxSensorDistance;
 		msg[i].min_range = obj.minSensorDistance;
-		msg[i].field_of_view = obj.sensorConeApperture;
+		msg[i].field_of_view = obj.sensorConeAperture;
+
+		// See: https://github.com/MRPT/mrpt/issues/1270
+		if constexpr (has_variance<sensor_msgs::msg::Range>::value)
+		{
+			msg[i].variance =
+				mrpt::square(obj.sensedData[i].sensorNoiseStdDeviation);
+		}
 	}
 
 	/// following part needs to be double checked, it looks incorrect
 	/// ROS has single number float for range, MRPT has a list of
 	/// sensedDistances
-	for (long i = 0; i < num_range; i++)
+	for (size_t i = 0; i < num_range; i++)
 		msg[i].range = obj.sensedData.at(i).sensedDistance;
 
 	/// currently the following are not available in MRPT for corresponding

python/src/mrpt/obs/CObservationRange.cpp

@@ -246,7 +246,7 @@ void bind_mrpt_obs_CObservationRange(std::function< pybind11::module &(std::stri
 		cl.def( pybind11::init( [](mrpt::obs::CObservationRange const &o){ return new mrpt::obs::CObservationRange(o); } ) );
 		cl.def_readwrite("minSensorDistance", &mrpt::obs::CObservationRange::minSensorDistance);
 		cl.def_readwrite("maxSensorDistance", &mrpt::obs::CObservationRange::maxSensorDistance);
-		cl.def_readwrite("sensorConeApperture", &mrpt::obs::CObservationRange::sensorConeApperture);
+		cl.def_readwrite("sensorConeAperture", &mrpt::obs::CObservationRange::sensorConeAperture);
 		cl.def_readwrite("sensedData", &mrpt::obs::CObservationRange::sensedData);
 		cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::obs::CObservationRange::GetRuntimeClassIdStatic, "C++: mrpt::obs::CObservationRange::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic);
 		cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::obs::CObservationRange::*)() const) &mrpt::obs::CObservationRange::GetRuntimeClass, "C++: mrpt::obs::CObservationRange::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic);

python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi

@@ -1217,7 +1217,7 @@ class CObservationRange(CObservation):
     maxSensorDistance: float
     minSensorDistance: float
     sensedData: Any
-    sensorConeApperture: float
+    sensorConeAperture: float
     @overload
     def __init__(self) -> None: ...
     @overload