complete global localization with gps and fused sensor data

uwrobotics · May 20, 2024 · 4ac35e7 · 4ac35e7
1 parent 17c4957
commit 4ac35e7
Show file tree

Hide file tree

Showing 5 changed files with 124 additions and 21 deletions.
diff --git a/...over_drivetrain/uwrt_mars_rover_drivetrain_description/config/dual_ekf_navsat_params.yaml b/...over_drivetrain/uwrt_mars_rover_drivetrain_description/config/dual_ekf_navsat_params.yaml
@@ -1,3 +1,6 @@
+# for more info on configuation see
+# 1. https://docs.nav2.org/tutorials/docs/navigation2_with_gps.html
+# 2. http://docs.ros.org/en/noetic/api/robot_localization/html/integrating_gps.html#configuring-robot-localization
 ekf_filter_node_odom:
   ros__parameters:
     frequency: 30.0
@@ -6,7 +9,7 @@ ekf_filter_node_odom:
     debug: false
     publish_tf: true
 
-    # map_frame: odom
+    map_frame: map
     odom_frame: odom
     base_link_frame: base_link # the frame id used by the diff drive plugin
     world_frame: odom
@@ -49,3 +52,77 @@ ekf_filter_node_odom:
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.3,   0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.3,   0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.3]
+
+ekf_filter_node_map:
+  ros__parameters:
+    frequency: 30.0
+    two_d_mode: true  # Recommended to use 2d mode for nav2 in mostly planar environments
+    print_diagnostics: true
+    debug: false
+    publish_tf: true
+
+    map_frame: map
+    odom_frame: odom
+    base_link_frame: base_link # the frame id used by the diff drive plugin
+    world_frame: map
+
+    odom0: odom
+    odom0_config: [false, false, false, # use the same config as above
+                  false, false, false,
+                  true,  true,  true,
+                  false, false, true,
+                  false, false, false]
+    odom0_queue_size: 10
+    odom0_differential: false
+    odom0_relative: false
+
+    odom1: odometry/gps
+    odom1_config: [true,  true,  false,
+                  false, false, false,
+                  false, false, false,
+                  false, false, false,
+                  false, false, false]
+    odom1_queue_size: 10
+    odom1_differential: false
+    odom1_relative: false
+
+    imu0: imu/data
+    imu0_config: [false, false, false,
+                  false,  false,  true,
+                  false, false, false,
+                  false,  false,  false,
+                  false,  false,  false]
+    imu0_differential: false  # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
+    imu0_relative: false
+    imu0_queue_size: 10
+    imu0_remove_gravitational_acceleration: true
+
+    use_control: false
+
+    process_noise_covariance: [1.0,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    1.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    1e-3,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.3,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.3,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.01,   0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.5,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.5,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.3,    0.0,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.3,    0.0,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.3,    0.0,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.3,    0.0,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.3,    0.0,
+                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.3]
+
+navsat_transform:
+  ros__parameters:
+    frequency: 30.0
+    delay: 3.0
+    magnetic_declination_radians: 0.0
+    yaw_offset: 0.0
+    zero_altitude: true
+    broadcast_cartesian_transform: true
+    publish_filtered_gps: true
+    use_odometry_yaw: true
+    wait_for_datum: false
diff --git a/..._rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/dual_ekf_navsat.launch.py b/..._rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/dual_ekf_navsat.launch.py
@@ -26,7 +26,33 @@ def generate_launch_description():
                 name="ekf_filter_node_odom",
                 output="screen",
                 parameters=[rl_params_file, {"use_sim_time": True}],
+                # first ekf publishes to odometry/local and gives odom -> base_link transform
                 remappings=[("odometry/filtered", "odometry/local")],
             ),
+            launch_ros.actions.Node(
+                package="robot_localization",
+                executable="ekf_node",
+                name="ekf_filter_node_map",
+                output="screen",
+                parameters=[rl_params_file, {"use_sim_time": True}],
+                # second ekf publishes to odometry/global and gives map -> odom transform
+                remappings=[("odometry/filtered", "odometry/global")],
+            ),
+            launch_ros.actions.Node(
+                package="robot_localization",
+                executable="navsat_transform_node",
+                name="navsat_transform",
+                output="screen",
+                parameters=[rl_params_file, {"use_sim_time": True}],
+                # this is the nav sat transform node which esentially changes the global GPS coordinates to 
+                # the local coordinates, so that it can be fused into localization
+                remappings=[
+                    ("imu/data", "imu/data"),
+                    ("gps/fix", "gps/fix"),
+                    ("gps/filtered", "gps/filtered"), # no need to remap these
+                    ("odometry/gps", "odometry/gps"),
+                    ("odometry/filtered", "odometry/global"),
+                ],
+            ),
         ]
     )
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py
@@ -15,20 +15,10 @@
 
 
 def generate_launch_description():
-    #this is launching gazebo
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(
-            get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
-        )
-
-    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
-                arguments=['-topic', 'robot_description',
-                            '-entity', 'my_bot'],
-                output='screen')
-    #-------------------------------------------------------------------------------------
-
-
+    gazebo_ros_package_path = get_package_share_directory('gazebo_ros')
     drivetrain_description_package_path = get_package_share_path('uwrt_mars_rover_drivetrain_description')
+    # TODO: change the world path below to an argument that we can change
+    my_world_path = drivetrain_description_package_path / 'world' / 'my_world.sdf'
     model_path = drivetrain_description_package_path / 'urdf' / 'drivetrain.urdf.xacro'
     rviz_config_path = drivetrain_description_package_path / 'rviz' / 'urdf.rviz'
     controllers_config_path = get_package_share_path(
@@ -37,6 +27,17 @@ def generate_launch_description():
     robot_description_content = ParameterValue(Command(['ros2 run xacro xacro ', str(model_path)]), value_type=str)
     robot_description = {'robot_description': robot_description_content}
 
+    # launch gazebo
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(gazebo_ros_package_path, 'launch', 'gazebo.launch.py')]),
+        launch_arguments={'world': str(my_world_path)}.items(),
+        )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                arguments=['-topic', 'robot_description',
+                            '-entity', 'my_bot'],
+                output='screen')
+
     # Nodes
     nodes = []
     nodes += [gazebo]
@@ -109,8 +110,6 @@ def generate_launch_description():
     #-------------------------------------------------------------------------------------
 
 
-
-
     # Run the node
     return LaunchDescription(
         nodes

diff --git a/..._mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro b/..._mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
@@ -261,7 +261,8 @@
                 <frame_name>gps_link</frame_name>
             </plugin>
             <always_on>true</always_on>
-            <update_rate>100</update_rate>
+            <!-- reduced update rate for the GPS; generally should publish at lower frequency -->
+            <update_rate>1</update_rate>
             <visualize>true</visualize>
             <gps>
 

diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/world/my_world.sdf b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/world/my_world.sdf
@@ -81,10 +81,10 @@
     <wind/>
     <spherical_coordinates>
       <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>