Add Noise to Slotcar Plugin

rmf_robot_sim_common/CMakeLists.txt

@@ -16,17 +16,22 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(eigen3_cmake_module REQUIRED)
-find_package(Eigen3 REQUIRED)
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(rmf_fleet_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rmf_building_map_msgs REQUIRED)
 find_package(rmf_dispenser_msgs REQUIRED)
+find_package(rmf_fleet_msgs REQUIRED)
 find_package(rmf_ingestor_msgs REQUIRED)
-find_package(rmf_building_map_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+find_package(ignition-cmake2 REQUIRED)
+ign_find_package(ignition-sensors6 REQUIRED)
+ign_find_package(ignition-common4 REQUIRED)
+ign_find_package(sdformat12 REQUIRED)
 
 include(GNUInstallDirs)
 
@@ -133,6 +138,7 @@ target_include_directories(readonly_common
 add_library(slotcar_common SHARED ${PROJECT_SOURCE_DIR}/src/slotcar_common.cpp)
 
 ament_target_dependencies(slotcar_common
+  PUBLIC
     Eigen3
     rmf_fleet_msgs
     rmf_building_map_msgs
@@ -147,10 +153,15 @@ target_include_directories(slotcar_common
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
     ${EIGEN3_INCLUDE_DIRS}
+    ${IGNITION-SENSORS_INCLUDE_DIRS}
+    ${IGNITION-COMMON_INCLUDE_DIRS}
 )
 
 target_link_libraries(slotcar_common
+  PUBLIC
     rmf_robot_sim_utils
+    #ignition-sensosr6::ignition-sensosr6
+    sdformat12::sdformat12
 )
 
 ###############################

rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp

@@ -18,6 +18,7 @@
 #ifndef RMF_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP
 #define RMF_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP
 
+#include <optional>
 #include <sstream>
 
 #include <rclcpp/rclcpp.hpp>
@@ -32,6 +33,8 @@
 #include <rmf_fleet_msgs/msg/mode_request.hpp>
 #include <rmf_building_map_msgs/msg/building_map.hpp>
 
+#include <ignition/sensors/Noise.hh>
+
 namespace rmf_robot_sim_common {
 
 struct SlotcarTrajectory
@@ -105,6 +108,20 @@ class SlotcarCommon
 
   void publish_robot_state(const double time);
 
+  /// Add noise to the reported location
+  /// \param[in] noise to be applied
+  void set_translation_noise(ignition::sensors::NoisePtr noise);
+
+  /// Add noise to the reported orientation
+  /// \param[in] noise to be applied
+  void set_rotation_noise(ignition::sensors::NoisePtr noise);
+
+  /// Stop using translation noise
+  void unset_translation_noise();
+
+  /// Stop using rotation noise
+  void unset_rotation_noise();
+
   Eigen::Vector3d get_lookahead_point() const;
 
   bool display_markers = false; // Ignition only: toggles display of waypoint and lookahead markers
@@ -115,6 +132,12 @@ class SlotcarCommon
   { _path_request_callback = cb; }
 
 private:
+  // Localization noise - Translation
+  std::optional<ignition::sensors::NoisePtr> _translation_noise;
+
+  // Localization noise - Rotation
+  std::optional<ignition::sensors::NoisePtr> _rotation_noise;
+
   // Parameters needed for power dissipation and charging calculations
   // Default values may be overriden using values from sdf file
   struct PowerParams
@@ -248,7 +271,7 @@ class SlotcarCommon
 
   void publish_tf2(const rclcpp::Time& t);
 
-  void publish_state_topic(const rclcpp::Time& t);
+  void publish_state_topic(const rclcpp::Time& t, double dt);
 
   bool path_request_valid(
     const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);

rmf_robot_sim_common/src/slotcar_common.cpp

@@ -929,6 +929,8 @@ void SlotcarCommon::publish_robot_state(const double time)
   const uint32_t t_nsec =
     static_cast<uint32_t>((time-static_cast<double>(t_sec)) *1e9);
   const rclcpp::Time ros_time{t_sec, t_nsec, RCL_ROS_TIME};
+  double dt = time - _last_update_time;
+
   if ((time - last_tf2_pub) > (1.0 / TF2_RATE))
   {
     // Publish tf2
@@ -938,11 +940,31 @@ void SlotcarCommon::publish_robot_state(const double time)
   if ((time - last_topic_pub) > (1.0 / STATE_TOPIC_RATE))
   {
     // Publish state topic
-    publish_state_topic(ros_time);
+    publish_state_topic(ros_time, dt);
     last_topic_pub = time;
   }
 }
 
+void SlotcarCommon::set_translation_noise(ignition::sensors::NoisePtr noise)
+{
+  _translation_noise = noise;
+}
+
+void SlotcarCommon::set_rotation_noise(ignition::sensors::NoisePtr noise)
+{
+  _rotation_noise = noise;
+}
+
+void SlotcarCommon::unset_translation_noise()
+{
+  _translation_noise = std::nullopt;
+}
+
+void SlotcarCommon::unset_rotation_noise()
+{
+  _rotation_noise = std::nullopt;
+}
+
 void SlotcarCommon::publish_tf2(const rclcpp::Time& t)
 {
   geometry_msgs::msg::TransformStamped tf_stamped;
@@ -960,7 +982,7 @@ void SlotcarCommon::publish_tf2(const rclcpp::Time& t)
   _tf2_broadcaster->sendTransform(tf_stamped);
 }
 
-void SlotcarCommon::publish_state_topic(const rclcpp::Time& t)
+void SlotcarCommon::publish_state_topic(const rclcpp::Time& t, double dt)
 {
   rmf_fleet_msgs::msg::RobotState robot_state_msg;
   robot_state_msg.name = _model_name;
@@ -972,6 +994,22 @@ void SlotcarCommon::publish_state_topic(const rclcpp::Time& t)
   robot_state_msg.location.t = t;
   robot_state_msg.location.level_name = get_level_name(_pose.translation()[2]);
 
+  // If noise enabled
+  if (_translation_noise.has_value())
+  {
+    robot_state_msg.location.x =
+      _translation_noise.value()->Apply(robot_state_msg.location.x, dt);
+    robot_state_msg.location.x =
+      _translation_noise.value()->Apply(robot_state_msg.location.y, dt);
+  }
+
+  if (_rotation_noise.has_value())
+  {
+    robot_state_msg.location.yaw =
+      _rotation_noise.value()->Apply(robot_state_msg.location.yaw, dt);
+  }
+
+
   if (robot_state_msg.location.level_name.empty())
   {
     RCLCPP_ERROR(

rmf_robot_sim_gz_classic_plugins/CMakeLists.txt

@@ -27,6 +27,8 @@ find_package(rmf_fleet_msgs REQUIRED)
 find_package(rmf_building_map_msgs REQUIRED)
 find_package(rmf_robot_sim_common REQUIRED)
 
+find_package(ignition-cmake2 REQUIRED)
+ign_find_package(sdformat12 REQUIRED)
 include(GNUInstallDirs)
 
 ###############################

rmf_robot_sim_gz_plugins/src/slotcar.cpp

@@ -1,3 +1,4 @@
+#include <typeinfo>
 #include <unordered_map>
 
 #include <ignition/plugin/Register.hh>
@@ -16,6 +17,7 @@
 
 #include <ignition/math/eigen3.hh>
 #include <ignition/msgs.hh>
+#include <ignition/sensors.hh>
 #include <ignition/transport.hh>
 #include <rclcpp/rclcpp.hpp>
 
@@ -86,6 +88,8 @@ class IGNITION_GAZEBO_VISIBLE SlotcarPlugin
 
   void draw_lookahead_marker();
 
+  void translation_noise_cb(const ignition::msgs::SensorNoise& msg);
+
   ignition::msgs::Marker_V _trajectory_marker_msg;
 };
 
@@ -99,12 +103,55 @@ SlotcarPlugin::SlotcarPlugin()
     std::cerr << "Error subscribing to topic [/charge_state]" << std::endl;
   }
   // We do rest of initialization during ::Configure
+
+  if (!_ign_node.Subscribe("/slotcar/translation_noise",
+    &SlotcarPlugin::translation_noise_cb,
+    this))
+  {
+    std::cerr << "Error subscribing to topic [/slotcar/translation_noise]" <<
+      std::endl;
+  }
 }
 
 SlotcarPlugin::~SlotcarPlugin()
 {
 }
 
+
+void SlotcarPlugin::translation_noise_cb(const ignition::msgs::SensorNoise& msg)
+{
+  if (msg.type() == 0)
+  {
+    dataPtr->unset_translation_noise();
+  }
+  else
+  {
+    sdf::Noise noise;
+    switch (msg.type())
+    {
+      case 2:
+        noise.SetType(sdf::NoiseType::GAUSSIAN);
+        break;
+      case 3:
+        noise.SetType(sdf::NoiseType::GAUSSIAN_QUANTIZED);
+        break;
+      default:
+        return;
+    }
+    noise.SetMean(msg.mean());
+    noise.SetStdDev(msg.stddev());
+    noise.SetBiasMean(msg.bias_mean());
+    noise.SetBiasStdDev(msg.bias_stddev());
+    noise.SetPrecision(msg.precision());
+    noise.SetDynamicBiasStdDev(msg.dynamic_bias_stddev());
+    noise.SetDynamicBiasCorrelationTime(msg.dynamic_bias_correlation_time());
+
+    auto noise_ptr = ignition::sensors::NoiseFactory::NewNoiseModel(noise);
+    dataPtr->set_translation_noise(noise_ptr);
+  }
+}
+
+
 void SlotcarPlugin::Configure(const Entity& entity,
   const std::shared_ptr<const sdf::Element>& sdf,
   EntityComponentManager& ecm, EventManager&)