Merge pull request #686 from JdeRobot/add-noise-to-odom

Add noise to odometry

Author: javizqh

CustomRobots/roomba_robot/models/roombaROS/model.sdf

@@ -9,8 +9,11 @@
       <inertial>
         <mass>1.0</mass>
         <inertia>
-          <ixx>1</ixx><ixy>0.0</ixy><ixz>0.0</ixz>
-          <iyy>1</iyy><iyz>0.0</iyz>
+          <ixx>1</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>1</iyy>
+          <iyz>0.0</iyz>
           <izz>1</izz>
         </inertia>
       </inertial>
@@ -294,7 +297,7 @@
 
     <link name="right_wheel">
       <pose>0 -0.130000 0.032000 0 0 0</pose>
-      
+
       <inertial>
         <inertia>
           <ixx>0.001000</ixx>
@@ -457,5 +460,14 @@
       <dimensions>3</dimensions>
     </plugin>
 
+    <plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
+      <odom_topic>/roombaROS/odom_noisy</odom_topic>
+      <robot_base_frame>base</robot_base_frame>
+      <odom_frame>odom</odom_frame>
+      <odom_publish_frequency>20</odom_publish_frequency>
+      <dimensions>3</dimensions>
+      <gaussian_noise>0.01</gaussian_noise>
+    </plugin>
+
   </model>
 </sdf>

CustomRobots/roomba_robot/models/roombaROS_cam/model.sdf

@@ -29,8 +29,11 @@
       <inertial>
         <mass>1.0</mass>
         <inertia>
-          <ixx>1</ixx><ixy>0.0</ixy><ixz>0.0</ixz>
-          <iyy>1</iyy><iyz>0.0</iyz>
+          <ixx>1</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>1</iyy>
+          <iyz>0.0</iyz>
           <izz>1</izz>
         </inertia>
       </inertial>
@@ -551,7 +554,7 @@
             <mean>0.0</mean>
             <stddev>0.007</stddev>
           </noise>
-        </camera> 
+        </camera>
       </sensor>
 
       <velocity_decay>
@@ -625,5 +628,14 @@
       <dimensions>3</dimensions>
     </plugin>
 
+    <plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
+      <odom_topic>/roombaROS/odom_noisy</odom_topic>
+      <robot_base_frame>base</robot_base_frame>
+      <odom_frame>odom</odom_frame>
+      <odom_publish_frequency>20</odom_publish_frequency>
+      <dimensions>3</dimensions>
+      <gaussian_noise>0.01</gaussian_noise>
+    </plugin>
+
   </model>
 </sdf>

CustomRobots/roomba_robot/params/roombaROS.yaml

@@ -5,6 +5,11 @@
   gz_type_name: "gz.msgs.Odometry"
   direction: GZ_TO_ROS
 
+- ros_topic_name: "odom_noisy"
+  gz_topic_name: "/roombaROS/odom_noisy"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS
 
 # gz topic subscribed to by DiffDrive plugin
 - ros_topic_name: "cmd_vel"
@@ -36,4 +41,4 @@
   gz_topic_name: "/roombaROS/events/left_bumper"
   ros_type_name: "ros_gz_interfaces/msg/Contacts"
   gz_type_name: "gz.msgs.Contacts"
-  direction: GZ_TO_ROS
+  direction: GZ_TO_ROS

CustomRobots/roomba_robot/params/roombaROS_cam.yaml

@@ -5,6 +5,11 @@
   gz_type_name: "gz.msgs.Odometry"
   direction: GZ_TO_ROS
 
+- ros_topic_name: "odom_noisy"
+  gz_topic_name: "/roombaROS/odom_noisy"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS
 
 # gz topic subscribed to by DiffDrive plugin
 - ros_topic_name: "cmd_vel"
@@ -43,4 +48,4 @@
   gz_topic_name: "/camera/camera_info"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
-  direction: GZ_TO_ROS
+  direction: GZ_TO_ROS

CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>TurtleBot3(Waffle)</name>
+  <version>2.0</version>
+  <sdf version="1.4">model-1_4.sdf</sdf>
+  <sdf version="1.5">model.sdf</sdf>
+
+  <author>
+    <name>Taehun Lim(Darby)</name>
+    <email>thlim@robotis.com</email>
+  </author>
+
+  <description>
+    TurtleBot3 Waffle
+  </description>
+</model>