Merge pull request #50 from clubcapra/46-pose-2-GPS

46 pose 2 gps

src/rove_bringup/launch/vectornav.launch.py

@@ -21,5 +21,9 @@ def generate_launch_description():
             executable='vn_sensor_msgs',
             output='screen',
             parameters=[vn_param_file],
+            remappings=[
+                ('/vectornav/gnss', '/gps'),
+                ('/vectornav/imu', '/imu'),
+            ]
         ),
     ])

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: "/gps"
+  gz_topic_name: "/navsat"
+  ros_type_name: "sensor_msgs/msg/NavSatFix"
+  gz_type_name: "ignition.msgs.NavSat"
+  direction: GZ_TO_ROS
+

src/rove_description/config/basic.rviz

@@ -83,7 +83,7 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        imu_link:
+        sensor_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -177,7 +177,7 @@ Visualization Manager:
           Value: true
         flipper_rr:
           Value: true
-        imu_link:
+        sensor_link:
           Value: true
         map:
           Value: true
@@ -207,7 +207,7 @@ Visualization Manager:
                   {}
                 flipper_rr:
                   {}
-                imu_link:
+                sensor_link:
                   {}
                 track_l:
                   {}

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: "gps"
+  gz_topic_name: "navsat"
+  ros_type_name: "sensor_msgs/msg/NavSatFix"
+  gz_type_name: "ignition.msgs.NavSat"
+  direction: GZ_TO_ROS

src/rove_description/config/navsat_transform.yaml

@@ -0,0 +1,12 @@
+# used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html
+navsat_transform:
+  ros__parameters:
+    frequency: 30.0
+    delay: 3.0
+    magnetic_declination_radians: 0.0
+    yaw_offset: 0.0
+    zero_altitude: true
+    broadcast_utm_transform: true
+    publish_filtered_gps: true
+    use_odometry_yaw: true
+    wait_for_datum: false 

src/rove_description/launch/amazon.launch.py

@@ -92,22 +92,54 @@ def generate_launch_description():
         }.items(),
     )
 
-    robot_localization_node = Node(
+    # used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html
+
+    robot_localization_node_local = Node(
        package='robot_localization',
        executable='ekf_node',
-       name='ekf_filter_node',
+       name='ekf_filter_node_local',
        output='screen',
        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
                    {'use_sim_time': True}]
                    )
 
+    robot_localization_node_global = Node(
+       package='robot_localization',
+       executable='ekf_node',
+       name='ekf_filter_node_global',
+       output='screen',
+       parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                   {'use_sim_time': True}]
+       remappings=[("odometry/filtered", "odometry/global")],
+    )
+
+    navsat_transform = Node(
+        package="robot_localization",
+        executable="navsat_transform_node",
+        name="navsat_transform",
+        output="screen",
+        parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'),
+                   {'use_sim_time': True}],
+        remappings=[
+            # Subscribed Topics
+                    ("imu/data", "imu"),
+                    ("gps/fix", "gps"),
+                    ("odometry/filtered", "odometry/filtered/global"),
+            # Published Topics
+                    ("gps/filtered", "gps/filtered"),
+                    ("odometry/gps", "odometry/gps"),
+                ],
+    )
+
     return LaunchDescription([
         gz_sim,
         DeclareLaunchArgument('rviz', default_value='true',
                               description='Open RViz.'),
         bridge,
         robot_state_publisher,
-        robot_localization_node,
+        robot_localization_node_local,
+        robot_localization_node_global,
+        navsat_transform,
         rviz,
         slam,
         create,

src/rove_description/launch/sim.launch.py

@@ -110,15 +110,45 @@ def generate_launch_description():
         )
     )
 
-    robot_localization_node = Node(
+    # used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html
+
+    robot_localization_node_local = Node(
        package='robot_localization',
        executable='ekf_node',
-       name='ekf_filter_node',
+       name='ekf_filter_node_local',
        output='screen',
        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
                    {'use_sim_time': True}]
                    )
 
+    robot_localization_node_global = Node(
+       package='robot_localization',
+       executable='ekf_node',
+       name='ekf_filter_node_global',
+       output='screen',
+       parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                   {'use_sim_time': True}],
+       remappings=[("odometry/filtered", "odometry/filtered/global")],
+    )
+
+    navsat_transform = Node(
+        package="robot_localization",
+        executable="navsat_transform_node",
+        name="navsat_transform",
+        output="screen",
+        parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'),
+                   {'use_sim_time': True}],
+        remappings=[
+            # Subscribed Topics
+                    ("imu/data", "imu"),
+                    ("gps/fix", "gps"),
+                    ("odometry/filtered", "odometry/filtered/global"),
+            # Published Topics
+                    ("gps/filtered", "gps/filtered"),
+                    ("odometry/gps", "odometry/gps"),
+                ],
+    )
+
     teleop = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(bringup_pkg_path, "launch", "rove_controller_usb.launch.py"),
@@ -131,7 +161,9 @@ def generate_launch_description():
                                   description='Open RViz.'),
             bridge,
             robot_state_publisher,
-            robot_localization_node,
+            robot_localization_node_local,
+            robot_localization_node_global,
+            navsat_transform,
             rviz,
             slam,
             #slam3d,

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>

src/rove_description/urdf/rove.urdf.xacro

@@ -12,7 +12,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/sensor.urdf.xacro" />
 
     <xacro:include filename="$(find rove_description)/urdf/flipper.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/track.urdf.xacro" />

src/rove_description/urdf/imu.urdf.xacro → src/rove_description/urdf/sensor.urdf.xacro

@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <joint name="imu_joint" type="fixed">
+    <joint name="sensor_joint" type="fixed">
         <parent link="base_link"/>
-        <child link="imu_link"/>
+        <child link="sensor_link"/>
         <origin xyz="0.1 0 0.212" rpy="0 0 0"/>
     </joint>
 
-    <link name="imu_link">
+    <link name="sensor_link">
         <inertial>
             <origin xyz="0 0 0" rpy="0 0 0"/>
             <mass value="0.1"/>
@@ -27,7 +27,7 @@
         </collision>
     </link>
 
-    <gazebo reference="imu_link">
+    <gazebo reference="sensor_link">
         <material>Gazebo/Black</material>
 
         <sensor name="imu_sensor" type="imu">
@@ -49,5 +49,12 @@
                 </noise>
             </imu>
         </sensor>
+
+        <sensor name="gps_sensor" type="navsat">
+            <gz_frame_id>sensor_link</gz_frame_id>
+            <always_on>true</always_on>
+            <update_rate>1</update_rate>
+            <topic>navsat</topic>
+        </sensor>
     </gazebo>
 </robot>

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>

src/rove_slam/config/ekf.yaml

@@ -1,5 +1,5 @@
 ### ekf config file ###
-ekf_filter_node:
+ekf_filter_node_local:
     ros__parameters:
         frequency: 30.0
         two_d_mode: false
@@ -24,3 +24,44 @@ ekf_filter_node:
                       false, false, false,
                       false, false, false,
                       false, false, false]
+
+ekf_filter_node_global:
+    ros__parameters:
+        frequency: 30.0
+        two_d_mode: false
+        publish_acceleration: true
+        publish_tf: true
+
+        map_frame: map              
+        odom_frame: odom            
+        base_link_frame: base_footprint
+        world_frame: map           
+
+        odom0: /odom
+        odom0_config: [true,  true,  true,
+                       false, false, true,
+                       false, false, false,
+                       false, false, true,
+                       false, false, false]
+
+        imu0: /imu
+        imu0_config: [false, false, false,
+                      true,  true,  true,
+                      false, false, false,
+                      false, false, false,
+                      false, false, false]
+
+        # description from https://github.com/cra-ros-pkg/robot_localization/blob/ros2/params/ekf.yaml
+        # Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+        # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+        # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+        # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+        # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+        # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+        # if unspecified, effectively making this parameter required for each sensor.
+        odom1: /odometry/gps
+        odom1_config: [true,  true,  true,
+                       false, false, false,
+                       false, false, false,
+                       false, false, false,
+                       false, false, false]