add gps sensor, gps navsat plugin , bridge topic for sam_bot custom robot

Author: star

sam_bot_description/config/bridge_config.yaml

@@ -26,9 +26,9 @@
   direction: GZ_TO_ROS
 
 # Topic subscribed to by DiffDrive plugin
-- ros_topic_name: "/demo/cmd_vel"
+- ros_topic_name: "/cmd_vel"
   gz_topic_name: "/demo/cmd_vel"
-  ros_type_name: "geometry_msgs/msg/TwistStamped"
+  ros_type_name: "geometry_msgs/msg/Twist"
   gz_type_name: "gz.msgs.Twist"
   direction: ROS_TO_GZ
 
@@ -55,3 +55,13 @@
   ros_type_name: "sensor_msgs/msg/PointCloud2"
   gz_type_name: "gz.msgs.PointCloudPacked"
   direction: GZ_TO_ROS
+
+- topic_name: "gps/fix"
+  ros_type_name: "sensor_msgs/msg/NavSatFix"
+  gz_type_name: "gz.msgs.NavSat"
+  direction: GZ_TO_ROS
+
+# - topic_name: "tf"
+#   ros_type_name: "tf2_msgs/msg/TFMessage"
+#   gz_type_name: "gz.msgs.Pose_V"
+#   direction: GZ_TO_ROS

sam_bot_description/src/description/sam_bot_description.sdf

@@ -188,7 +188,6 @@
       <child>imu_link</child>
       <pose relative_to="base_link">0.0 0.0 0.01 0 0 0</pose>
     </joint>
-
     <link name='imu_link'>
       <pose relative_to="imu_joint"/>
       <visual name="imu_link_visual">
@@ -271,7 +270,50 @@
         </imu>
       </sensor>
     </link>
-
+    <joint name='gps_joint' type='fixed'>
+      <parent>base_link</parent>
+      <child>gps_link</child>
+      <pose relative_to="base_link">0.0 0.0 0.3 0 0 0</pose>
+    </joint>
+    <link name="gps_link">
+      <!-- Some weird bug occurs if this joint gets the default inertia -->
+        <inertial>
+          <pose>-0.052 0 0.111 0 0 0</pose>
+          <inertia>
+            <ixx>0.001</ixx>
+            <ixy>0.000</ixy>
+            <ixz>0.000</ixz>
+            <iyy>0.001</iyy>
+            <iyz>0.000</iyz>
+            <izz>0.001</izz>
+          </inertia>
+          <mass>0.125</mass>
+        </inertial>
+        <sensor name="tb3_gps" type="gps">
+          <always_on>true</always_on>
+          <update_rate>1</update_rate>
+          <pose>0 0 0 0 0 0</pose>
+          <topic>gps/fix</topic>
+          <gz_frame_id>gps_link</gz_frame_id>
+          <gps>
+            <position_sensing>
+              <horizontal>
+                <noise type="gaussian">
+                  <mean>0.0</mean>
+                  <stddev>0.01</stddev>
+                </noise>
+              </horizontal>
+              <vertical>
+                <noise type="gaussian">
+                  <mean>0.0</mean>
+                  <stddev>0.01</stddev>
+                </noise>
+              </vertical>
+            </position_sensing>
+          </gps>
+        </sensor>
+    </link>
+  
     <plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
       <!-- wheels -->
       <left_joint>drivewhl_l_joint</left_joint>
@@ -349,11 +391,11 @@
             <max>3.5</max>
             <resolution>0.015000</resolution>
           </range>
-          <noise>
+          <!-- <noise>
             <type>gaussian</type>
             <mean>0.0</mean>
             <stddev>0.01</stddev>
-          </noise>
+          </noise> -->
         </ray>
       </sensor>
     </link>

sam_bot_description/world/my_world.sdf

@@ -24,6 +24,10 @@
       name="gz::sim::systems::Sensors">
       <render_engine>ogre2</render_engine>
     </plugin>
+    <plugin
+        filename="gz-sim-navsat-system"
+        name="gz::sim::systems::NavSat">
+    </plugin>
     <light name='sun' type='directional'>
       <cast_shadows>1</cast_shadows>
       <pose>0 0 10 0 -0 0</pose>
@@ -104,11 +108,15 @@
     </scene>
     <wind/>
     <spherical_coordinates>
+      <!-- currently gazebo has a bug: instead of outputing lat, long, altitude in ENU
+      (x = East, y = North and z = Up) as the default configurations, it's outputting (-E)(-N)U,
+      therefore we rotate the default frame 180 so that it would go back to ENU 
+      see: https://github.com/osrf/gazebo/issues/2022 --> 
       <surface_model>EARTH_WGS84</surface_model>
-      <latitude_deg>0</latitude_deg>
-      <longitude_deg>0</longitude_deg>
-      <elevation>0</elevation>
-      <heading_deg>0</heading_deg>
+      <latitude_deg>38.161479</latitude_deg>
+      <longitude_deg>-122.454630</longitude_deg>
+      <elevation>488.0</elevation>
+      <heading_deg>180</heading_deg>
     </spherical_coordinates>
     <model name='unit_box'>
       <pose>1.51271 -0.181418 0.5 0 -0 0</pose>