diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 4fc71755..e7204b00 100644 --- a/ros_gz_bridge/README.md +++ b/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 | diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp index 37b972ba..426894a0 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -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( diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 1b128db9..f87b94bc 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/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'), diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index f6e6e5f3..8da4225c 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/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( diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index a80e5325..4c10982b 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -266,6 +266,33 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_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 & _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(_msg->header())); +} + void createTestMsg(gz::msgs::Param & _msg) { createTestMsg(*_msg.mutable_header()); diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 0113ca35..aa4ded44 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -160,6 +161,14 @@ void createTestMsg(gz::msgs::Clock & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(gz::msgs::StringMsg & _msg); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index b0fc6d2f..6f1e2bed 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -657,6 +657,33 @@ void compareTestMsg(const std::shared_ptr & _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 & _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); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index 3fab7a8f..69dfd155 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -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 & _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 & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ros_gz_interfaces::msg::StringVec & _msg); diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt index ae656dd5..49c49126 100644 --- a/ros_gz_interfaces/CMakeLists.txt +++ b/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" diff --git a/ros_gz_interfaces/msg/SensorNoise.msg b/ros_gz_interfaces/msg/SensorNoise.msg new file mode 100644 index 00000000..0b1e5cec --- /dev/null +++ b/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 + +