diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml
index 75746820..8cc4055c 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml
@@ -176,7 +176,8 @@ map_saver:
map_server:
ros__parameters:
- yaml_filename: "/home/meshva/uwrt/uwrt_mars_rover/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/map.yaml"
+ # TODO: Change name "nico" to use the USER env var instead
+ yaml_filename: "/home/nico/uwrt_ws/src/uwrt_mars_rover/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/map.yaml"
topic_name: "map"
frame_id: "odom"
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/dual_ekf_navsat_params.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/dual_ekf_navsat_params.yaml
new file mode 100644
index 00000000..905bf5e0
--- /dev/null
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/dual_ekf_navsat_params.yaml
@@ -0,0 +1,51 @@
+ekf_filter_node_odom:
+ 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: odom
+ odom_frame: odom
+ base_link_frame: base_link # the frame id used by the diff drive plugin
+ world_frame: odom
+
+ odom0: odom
+ odom0_config: [false, false, false, # TODO: test switching this row to all true
+ false, false, false,
+ true, true, true, # start with the wheel velocities; try switching to positions later and see what works better
+ false, false, true,
+ false, false, false]
+ odom0_queue_size: 10
+ odom0_differential: false
+ odom0_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: [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, 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, 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]
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/dual_ekf_navsat.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/dual_ekf_navsat.launch.py
new file mode 100644
index 00000000..1c2ae75a
--- /dev/null
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/dual_ekf_navsat.launch.py
@@ -0,0 +1,32 @@
+from launch import LaunchDescription
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import launch_ros.actions
+import os
+import launch.actions
+
+
+def generate_launch_description():
+ gps_wpf_dir = os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'))
+ rl_params_file = os.path.join(
+ gps_wpf_dir, "config", "dual_ekf_navsat_params.yaml")
+
+ return LaunchDescription(
+ [
+ launch.actions.DeclareLaunchArgument(
+ "output_final_position", default_value="false"
+ ),
+ launch.actions.DeclareLaunchArgument(
+ "output_location", default_value="~/dual_ekf_navsat_example_debug.txt"
+ ),
+ launch_ros.actions.Node(
+ package="robot_localization",
+ executable="ekf_node",
+ name="ekf_filter_node_odom",
+ output="screen",
+ parameters=[rl_params_file, {"use_sim_time": True}],
+ remappings=[("odometry/filtered", "odometry/local")],
+ ),
+ ]
+ )
\ No newline at end of file
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py
index fbb95edb..807da2cb 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py
@@ -10,7 +10,14 @@
def generate_launch_description():
- declared_arguments = []
+ declared_arguments = [
+ # TODO: add this back after fixing the map path with the USER env var
+ # DeclareLaunchArgument(
+ # "map_yaml_filename",
+ # default_value=os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'), 'config', 'map.yaml'),
+ # description='Map yaml file path'
+ # )
+ ]
nodes = []
controller_yaml = os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'), 'config', 'costmap_parameters.yaml')
@@ -23,7 +30,10 @@ def generate_launch_description():
package='nav2_map_server',
executable='map_server',
output='screen',
- parameters=[controller_yaml])]
+ # TODO: add this back after fixing the map path with the USER env var
+ # parameters=[{"yaml_file": LaunchConfiguration("map_yaml_filename")}, controller_yaml],
+ parameters=[controller_yaml]
+ )]
nodes += [Node(
package='nav2_controller',
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
index c43b8176..ddcdfa5f 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
@@ -38,7 +38,7 @@
ixx="1e-3" ixy="0.0" ixz="0.0"
iyy="1e-3" iyz="0.0"
izz="1e-3"/>
-
+
@@ -134,4 +134,166 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ~/out:=imu/data
+
+ false
+
+ true
+ 100
+ true
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 0 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ~/out:=gps/fix
+
+ gps_link
+
+ true
+ 100
+ true
+
+
+
+ 0.0
+ 0.01
+
+
+
+
+ 0.0
+ 0.01
+
+
+
+
+
+ Gazebo/Grey
+
+
+
+
+
+
+
+ 0 0 1
+
+
+
+
+
+
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
index 7955cbcd..9d5ea2cb 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
@@ -47,7 +47,10 @@
-
+
+
+
+
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml
index 7bc23a44..3788b93b 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml
@@ -28,7 +28,7 @@ differential_drivetrain_controller:
twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.01 ]
publish_rate: 50.0
- enable_odom_tf: true
+ enable_odom_tf: false
open_loop: false # If true, integrates vel_cmd to estimate odom, else uses wheel feedback to compute odom
position_feedback: true # If true, uses position state_interface for odom calculations, else use velocity state interface
# TODO: this setting doesnt really make sense to me to tie both odom pose and twist calculations to either use position or