diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index e5e26efda8..950f7a5417 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/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)). diff --git a/libs/hwdrivers/src/CBoardSonars.cpp b/libs/hwdrivers/src/CBoardSonars.cpp index fa3b8e1bbf..9d60a16fd5 100644 --- a/libs/hwdrivers/src/CBoardSonars.cpp +++ b/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; diff --git a/libs/hwdrivers/src/CPhidgetInterfaceKitProximitySensors.cpp b/libs/hwdrivers/src/CPhidgetInterfaceKitProximitySensors.cpp index 2670e35526..f38a06af2e 100644 --- a/libs/hwdrivers/src/CPhidgetInterfaceKitProximitySensors.cpp +++ b/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(); diff --git a/libs/maps/src/maps/COccupancyGridMap2D_insert.cpp b/libs/maps/src/maps/COccupancyGridMap2D_insert.cpp index 7eb230414b..b5128d8f30 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_insert.cpp +++ b/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 diff --git a/libs/maps/src/maps/COccupancyGridMap2D_simulate.cpp b/libs/maps/src/maps/COccupancyGridMap2D_simulate.cpp index 4892711233..c344af0c88 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_simulate.cpp +++ b/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) diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index 92b6c7df9f..aeeb1ce762 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -1847,7 +1847,7 @@ bool CPointsMap::internal_insertObservation( const auto& o = static_cast(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) diff --git a/libs/obs/include/mrpt/obs/CObservationRange.h b/libs/obs/include/mrpt/obs/CObservationRange.h index 133260d2f5..269a7ba332 100644 --- a/libs/obs/include/mrpt/obs/CObservationRange.h +++ b/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; diff --git a/libs/obs/src/CObservationRange.cpp b/libs/obs/src/CObservationRange.cpp index a36c23db8d..4fe6422f0b 100644 --- a/libs/obs/src/CObservationRange.cpp +++ b/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"; } } diff --git a/libs/ros1bridge/src/range.cpp b/libs/ros1bridge/src/range.cpp index 49ae5c3ad7..5fe6d958ba 100644 --- a/libs/ros1bridge/src/range.cpp +++ b/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 diff --git a/libs/ros2bridge/src/range.cpp b/libs/ros2bridge/src/range.cpp index d2606320db..d33dd55fa6 100644 --- a/libs/ros2bridge/src/range.cpp +++ b/libs/ros2bridge/src/range.cpp @@ -15,16 +15,34 @@ #include +#include + +template +struct has_variance : std::false_type +{ +}; + +template +struct has_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::value) + { + obj.sensedData.at(0).sensorNoiseStdDeviation = std::sqrt(msg.variance); + } + return true; } @@ -32,24 +50,31 @@ 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::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 diff --git a/python/src/mrpt/obs/CObservationRange.cpp b/python/src/mrpt/obs/CObservationRange.cpp index f9508b6942..9d3399bd33 100644 --- a/python/src/mrpt/obs/CObservationRange.cpp +++ b/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); diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi index 39e5450df4..06867d0670 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi +++ b/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