Add gps sensor to sumulation, remap /vectornav/gnss to /gnss

clubcapra · May 1, 2024 · 9230247 · 9230247
1 parent 89d7d5f
commit 9230247
Show file tree

Hide file tree

Showing 8 changed files with 71 additions and 0 deletions.
diff --git a/src/rove_bringup/launch/vectornav.launch.py b/src/rove_bringup/launch/vectornav.launch.py
@@ -21,5 +21,8 @@ def generate_launch_description():
             executable='vn_sensor_msgs',
             output='screen',
             parameters=[vn_param_file],
+            remappings=[
+                ('/vectornav/gnss', '/gnss'),
+            ]
         ),
     ])
diff --git a/src/rove_description/config/amazon_bridge.yaml b/src/rove_description/config/amazon_bridge.yaml
@@ -39,3 +39,9 @@
   ros_type_name: "sensor_msgs/msg/Imu"
   gz_type_name: "gz.msgs.IMU"
   direction: GZ_TO_ROS
+- ros_topic_name: "/gnss"
+  gz_topic_name: "/navsat"
+  ros_type_name: "sensor_msgs/msg/NavSatFix"
+  gz_type_name: "ignition.msgs.NavSat"
+  direction: GZ_TO_ROS
+
diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz
@@ -209,6 +209,8 @@ Visualization Manager:
                   {}
                 imu_link:
                   {}
+                gps_link:
+                  {}
                 track_l:
                   {}
                 track_r:

diff --git a/src/rove_description/config/default_bridge.yaml b/src/rove_description/config/default_bridge.yaml
@@ -46,3 +46,10 @@
   ros_type_name: "sensor_msgs/msg/Imu"
   gz_type_name: "gz.msgs.IMU"
   direction: GZ_TO_ROS
+
+# gz topic published by NAVSAT plugin
+- ros_topic_name: "gnss"
+  gz_topic_name: "navsat"
+  ros_type_name: "sensor_msgs/msg/NavSatFix"
+  gz_type_name: "ignition.msgs.NavSat"
+  direction: GZ_TO_ROS
diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro
@@ -45,5 +45,7 @@
             name="gz::sim::systems::Sensors">
             <render_engine>ogre2</render_engine>
         </plugin>
+
+        <plugin filename="gz-sim-navsat-system" name="gz::sim::systems::NavSat"/>
     </gazebo>
 </robot>
diff --git a/src/rove_description/urdf/gps.urdf.xacro b/src/rove_description/urdf/gps.urdf.xacro
@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <joint name="gps_joint" type="fixed">
+        <parent link="base_link"/>
+        <child link="gps_link"/>
+        <origin xyz="0.1 0 0.212" rpy="0 0 0"/>
+    </joint>
+
+    <link name="gps_link">
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.1"/>
+            <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.1" radius="0.1"/>
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.1" radius="0.1"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <gazebo reference="gps_link">
+        <material>Gazebo/Black</material>
+        <sensor name="navsat" type="navsat">
+            <gz_frame_id>gps_link</gz_frame_id>
+            <always_on>true</always_on>
+            <update_rate>1</update_rate>
+            <topic>navsat</topic>
+        </sensor>
+    </gazebo>
+</robot>
diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
@@ -13,6 +13,7 @@
     <xacro:include filename="$(find rove_description)/urdf/gazebo.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/lidar.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/imu.urdf.xacro" />
+    <xacro:include filename="$(find rove_description)/urdf/gps.urdf.xacro" />
 
     <xacro:include filename="$(find rove_description)/urdf/flipper.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/track.urdf.xacro" />

diff --git a/src/rove_description/worlds/base_world.world b/src/rove_description/worlds/base_world.world
@@ -14,6 +14,17 @@
             </camera>
         </gui>
 
+        <spherical_coordinates>
+            <!-- currently gazebo has a bug: instead of outputting 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>38.161479</latitude_deg>
+            <longitude_deg>-122.454630</longitude_deg>
+            <elevation>488.0</elevation>
+            <heading_deg>180</heading_deg>
+        </spherical_coordinates>
 
         <physics name='1ms' type='ignored'>
             <max_step_size>0.001</max_step_size>