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 8cc4055c..44b41ccb 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 @@ -85,12 +85,17 @@ global_costmap: footprint_padding: 0.03 update_frequency: 5.0 publish_frequency: 5.0 - global_frame: odom + global_frame: map robot_base_frame: base_link use_sim_time: True - robot_radius: 0.22 # should modify - resolution: 0.05 - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + robot_radius: 0.95 # based on urdf + resolution: 0.1 # 0.1m per pixel of costmap; rough + rolling_window: true + width: 50 + height: 50 + track_unknown_space: true + # TODO: investigate the usage of a static layer... are we going to be mapping at all? + plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -104,18 +109,18 @@ global_costmap: marking: True data_type: "PointCloud2" min_obstacle_height: 0.001 - max_obstacle_height: 10.0 - denoise_layer: - plugin: "nav2_costmap_2d::DenoiseLayer" - enabled: True - minimal_group_size: 2 - group_connectivity_type: 8 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - enabled: true - subscribe_to_updates: true - transform_tolerance: 0.1 + max_obstacle_height: 3.0 + # denoise_layer: + # plugin: "nav2_costmap_2d::DenoiseLayer" + # enabled: True + # minimal_group_size: 2 + # group_connectivity_type: 8 + # static_layer: + # plugin: "nav2_costmap_2d::StaticLayer" + # map_subscribe_transient_local: True + # enabled: true + # subscribe_to_updates: true + # transform_tolerance: 0.1 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: true @@ -135,11 +140,12 @@ local_costmap: robot_base_frame: base_link use_sim_time: True rolling_window: true - width: 3 + width: 3 # local costmap is quite small height: 3 - resolution: 0.05 - - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + resolution: 0.05 # more fine precision map + # based off the current urdf file + footprint: "[ [0.735, 0.59], [0.735, -0.59], [-0.735, -0.59], [-0.735, 0.59] ]" + plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -153,13 +159,13 @@ local_costmap: marking: True data_type: "PointCloud2" min_obstacle_height: 0.001 - max_obstacle_height: 10.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - enabled: true - subscribe_to_updates: true - transform_tolerance: 0.1 + max_obstacle_height: 3.0 + # static_layer: + # plugin: "nav2_costmap_2d::StaticLayer" + # map_subscribe_transient_local: True + # enabled: true + # subscribe_to_updates: true + # transform_tolerance: 0.1 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: true @@ -174,12 +180,12 @@ map_saver: free_thresh_default: 0.25 occupied_thresh_default: 0.65 -map_server: - ros__parameters: - # 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" +# map_server: +# ros__parameters: +# # TODO: Change name "nico" to use the USER env var instead +# yaml_filename: "map.yaml" +# topic_name: "map" +# frame_id: "odom" costmap_filter_info_server: ros__parameters: 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 807da2cb..74e5f76e 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 @@ -2,7 +2,8 @@ from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.descriptions import ParameterFile @@ -11,29 +12,43 @@ def generate_launch_description(): declared_arguments = [ - # TODO: add this back after fixing the map path with the USER env var + # TODO: add this back after fixing the map path (if we need a static map) with the USER env var (if needed) # 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' # ) ] + + ekf_launch_file = os.path.join( + get_package_share_directory('uwrt_mars_rover_drivetrain_description'), + 'launch', + 'dual_ekf_navsat.launch.py' + ) + + ekf_launch = [IncludeLaunchDescription( + PythonLaunchDescriptionSource( + ekf_launch_file + ) + )] + nodes = [] - controller_yaml = os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'), 'config', 'costmap_parameters.yaml') + nav_config_yaml = os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'), 'config', 'costmap_parameters.yaml') lifecycle_nodes = ['controller_server', 'planner_server', - 'map_server'] + ] + # TODO: add this back after fixing the map path with the USER env var + # 'map_server'] - nodes += [Node( - package='nav2_map_server', - executable='map_server', - output='screen', - # 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_map_server', + # executable='map_server', + # output='screen', + # # parameters=[{"yaml_file": LaunchConfiguration("map_yaml_filename")}, nav_config_yaml], + # parameters=[nav_config_yaml] + # )] nodes += [Node( package='nav2_controller', @@ -41,7 +56,7 @@ def generate_launch_description(): output='screen', respawn=True, respawn_delay=2.0, - parameters=[controller_yaml])] + parameters=[nav_config_yaml])] nodes += [Node( package='nav2_planner', @@ -50,7 +65,7 @@ def generate_launch_description(): output='screen', respawn=True, respawn_delay=2.0, - parameters=[controller_yaml])] + parameters=[nav_config_yaml])] nodes += [Node( package='nav2_lifecycle_manager', @@ -60,4 +75,4 @@ def generate_launch_description(): parameters=[{'autostart': True}, {'node_names': lifecycle_nodes}])] - return LaunchDescription(declared_arguments + nodes) + return LaunchDescription(ekf_launch + declared_arguments + nodes)