SensorNoise msg bridging

ros_gz_bridge/README.md

@@ -34,6 +34,7 @@ The following message types can be bridged for topics:
 | ros_gz_interfaces/msg/GuiCamera              | gz.msgs.GUICamera              |
 | ros_gz_interfaces/msg/JointWrench            | gz.msgs.JointWrench            |
 | ros_gz_interfaces/msg/Light                  | gz.msgs.Light                  |
+| ros_gz_interfaces/msg/SensorNoise            | gz.msgs.SensorNoise            |
 | ros_gz_interfaces/msg/StringVec              | gz.msgs.StringMsg_V            |
 | ros_gz_interfaces/msg/TrackVisual            | gz.msgs.TrackVisual            |
 | ros_gz_interfaces/msg/VideoRecord            | gz.msgs.VideoRecord            |

ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp

@@ -27,6 +27,7 @@
 #include <gz/msgs/light.pb.h>
 #include <gz/msgs/param.pb.h>
 #include <gz/msgs/param_v.pb.h>
+#include <gz/msgs/sensor_noise.pb.h>
 #include <gz/msgs/stringmsg_v.pb.h>
 #include <gz/msgs/track_visual.pb.h>
 #include <gz/msgs/video_record.pb.h>
@@ -43,6 +44,7 @@
 #include <ros_gz_interfaces/msg/gui_camera.hpp>
 #include <ros_gz_interfaces/msg/light.hpp>
 #include <ros_gz_interfaces/msg/param_vec.hpp>
+#include <ros_gz_interfaces/msg/sensor_noise.hpp>
 #include <ros_gz_interfaces/msg/string_vec.hpp>
 #include <ros_gz_interfaces/msg/track_visual.hpp>
 #include <ros_gz_interfaces/msg/video_record.hpp>
@@ -151,6 +153,18 @@ convert_gz_to_ros(
   const gz::msgs::Light & gz_msg,
   ros_gz_interfaces::msg::Light & ros_msg);
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::SensorNoise & ros_msg,
+  gz::msgs::SensorNoise & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::SensorNoise & gz_msg,
+  ros_gz_interfaces::msg::SensorNoise & ros_msg);
+
 template<>
 void
 convert_ros_to_gz(

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -69,6 +69,7 @@
         Mapping('Light', 'Light'),
         Mapping('ParamVec', 'Param'),
         Mapping('ParamVec', 'Param_V'),
+        Mapping('SensorNoise', 'SensorNoise'),
         Mapping('StringVec', 'StringMsg_V'),
         Mapping('TrackVisual', 'TrackVisual'),
         Mapping('VideoRecord', 'VideoRecord'),

ros_gz_bridge/src/convert/ros_gz_interfaces.cpp

@@ -378,6 +378,53 @@ convert_gz_to_ros(
   ros_msg.intensity = gz_msg.intensity();
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::SensorNoise & ros_msg,
+  gz::msgs::SensorNoise & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, *gz_msg.mutable_header());
+  if (ros_msg.type == 0) {
+    gz_msg.set_type(gz::msgs::SensorNoise_Type::SensorNoise_Type_NONE);
+  } else if (ros_msg.type == 2) {
+    gz_msg.set_type(gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN);
+  } else if (ros_msg.type == 3) {
+    gz_msg.set_type(gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED);
+  }
+
+  gz_msg.set_mean(ros_msg.mean);
+  gz_msg.set_stddev(ros_msg.stddev);
+  gz_msg.set_bias_mean(ros_msg.bias_mean);
+  gz_msg.set_bias_stddev(ros_msg.bias_stddev);
+  gz_msg.set_precision(ros_msg.precision);
+  gz_msg.set_dynamic_bias_stddev(ros_msg.dynamic_bias_stddev);
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::SensorNoise & gz_msg,
+  ros_gz_interfaces::msg::SensorNoise & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+
+  if (gz_msg.type() == gz::msgs::SensorNoise_Type::SensorNoise_Type_NONE) {
+    ros_msg.type = 0;
+  } else if (gz_msg.type() == gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN) {
+    ros_msg.type = 2;
+  } else if (gz_msg.type() == gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED) {
+    ros_msg.type = 3;
+  }
+
+  ros_msg.mean = gz_msg.mean();
+  ros_msg.stddev = gz_msg.stddev();
+  ros_msg.bias_mean = gz_msg.bias_mean();
+  ros_msg.bias_stddev = gz_msg.bias_stddev();
+  ros_msg.precision = gz_msg.precision();
+  ros_msg.dynamic_bias_stddev = gz_msg.dynamic_bias_stddev();
+}
+
 template<>
 void
 convert_ros_to_gz(

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -266,6 +266,33 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::Altimeter> & _msg)
   compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
 }
 
+void createTestMsg(gz::msgs::SensorNoise & _msg)
+{
+  createTestMsg(*_msg.mutable_header());
+  _msg.set_type(gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED);
+  _msg.set_mean(100);
+  _msg.set_stddev(200);
+  _msg.set_bias_mean(300);
+  _msg.set_bias_stddev(400);
+  _msg.set_precision(500);
+  _msg.set_dynamic_bias_stddev(600);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::SensorNoise> & _msg)
+{
+  gz::msgs::SensorNoise expected_msg;
+  createTestMsg(expected_msg);
+
+  EXPECT_EQ(expected_msg.type(), gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED);
+  EXPECT_EQ(expected_msg.mean(), _msg->mean());
+  EXPECT_EQ(expected_msg.stddev(), _msg->stddev());
+  EXPECT_EQ(expected_msg.bias_mean(), _msg->bias_mean());
+  EXPECT_EQ(expected_msg.bias_stddev(), _msg->bias_stddev());
+  EXPECT_EQ(expected_msg.precision(), _msg->precision());
+  EXPECT_EQ(expected_msg.dynamic_bias_stddev(), _msg->dynamic_bias_stddev());
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+}
+
 void createTestMsg(gz::msgs::Param & _msg)
 {
   createTestMsg(*_msg.mutable_header());

ros_gz_bridge/test/utils/gz_test_msg.hpp

@@ -54,6 +54,7 @@
 #include <gz/msgs/pose_with_covariance.pb.h>
 #include <gz/msgs/pose_v.pb.h>
 #include <gz/msgs/quaternion.pb.h>
+#include <gz/msgs/sensor_noise.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
 #include <gz/msgs/stringmsg_v.pb.h>
 #include <gz/msgs/track_visual.pb.h>
@@ -160,6 +161,14 @@ void createTestMsg(gz::msgs::Clock & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::Clock> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::SensorNoise & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::SensorNoise> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(gz::msgs::StringMsg & _msg);

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -657,6 +657,33 @@ void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::ParamVec> & _m
   EXPECT_EQ(expected_msg.params[0].value.string_value, _msg->params[0].value.string_value);
 }
 
+void createTestMsg(ros_gz_interfaces::msg::SensorNoise & _msg)
+{
+  createTestMsg(_msg.header);
+
+  _msg.type = 3;
+  _msg.mean = 100;
+  _msg.stddev = 200;
+  _msg.bias_mean = 300;
+  _msg.bias_stddev = 400;
+  _msg.precision = 500;
+  _msg.dynamic_bias_stddev = 600;
+}
+
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::SensorNoise> & _msg)
+{
+  ros_gz_interfaces::msg::SensorNoise expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(_msg->header);
+  EXPECT_EQ(expected_msg.mean, _msg->mean);
+  EXPECT_EQ(expected_msg.stddev, _msg->stddev);
+  EXPECT_EQ(expected_msg.bias_mean, _msg->bias_mean);
+  EXPECT_EQ(expected_msg.bias_stddev, _msg->bias_stddev);
+  EXPECT_EQ(expected_msg.precision, _msg->precision);
+  EXPECT_EQ(expected_msg.dynamic_bias_stddev, _msg->dynamic_bias_stddev);
+}
+
 void createTestMsg(ros_gz_interfaces::msg::StringVec & _msg)
 {
   createTestMsg(_msg.header);

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -57,6 +57,7 @@
 #include <ros_gz_interfaces/msg/dataframe.hpp>
 #include <ros_gz_interfaces/msg/light.hpp>
 #include <ros_gz_interfaces/msg/param_vec.hpp>
+#include <ros_gz_interfaces/msg/sensor_noise.hpp>
 #include <ros_gz_interfaces/msg/string_vec.hpp>
 #include <ros_gz_interfaces/msg/track_visual.hpp>
 #include <ros_gz_interfaces/msg/video_record.hpp>
@@ -444,6 +445,14 @@ void createTestMsg(ros_gz_interfaces::msg::ParamVec & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::ParamVec> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(ros_gz_interfaces::msg::SensorNoise & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::SensorNoise> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ros_gz_interfaces::msg::StringVec & _msg);

ros_gz_interfaces/CMakeLists.txt

@@ -28,6 +28,7 @@ set(msg_files
   "msg/JointWrench.msg"
   "msg/Light.msg"
   "msg/ParamVec.msg"
+  "msg/SensorNoise.msg"
   "msg/StringVec.msg"
   "msg/TrackVisual.msg"
   "msg/VideoRecord.msg"

ros_gz_interfaces/msg/SensorNoise.msg

@@ -0,0 +1,43 @@
+# A message for specifying sensor noise.
+
+# Noise type
+uint8 NONE               = 0
+uint8 GAUSSIAN           = 2
+uint8 GAUSSIAN_QUANTIZED = 3
+
+# Optional header data.
+std_msgs/Header header
+
+# The type of noise
+uint8 type
+
+# Noise mean
+# Used by GAUSSIAN and GAUSSIAN_QUANTIZED
+float64 mean
+
+# Noise standard deviation
+# Used by GAUSSIAN and GAUSSIAN_QUANTIZED
+float64 stddev
+
+# Noise mean bias
+# Used by GAUSSIAN and GAUSSIAN_QUANTIZED
+float64 bias_mean
+
+# Noise standard deviation bias
+float64 bias_stddev
+
+# Noise precision
+# Used by GAUSSIAN_QUANTIZED
+float64 precision
+
+# For type "gaussian*", the standard deviation of the noise used to
+# drive a process to model slow variations in a sensor bias.
+float64 dynamic_bias_stddev
+
+# For type "gaussian*", the correlation time in seconds of the
+# noise used to drive a process to model slow variations in a sensor bias.
+# A typical value, when used, would be on the order of
+# 3600 seconds (1 hour).
+float64 dynamic_bias_correlation_time
+
+