diff --git a/navyu_navigation/config/navyu_params.yaml b/navyu_navigation/config/navyu_params.yaml new file mode 100644 index 0000000..668aa89 --- /dev/null +++ b/navyu_navigation/config/navyu_params.yaml @@ -0,0 +1,46 @@ +navyu_global_costmap_node: + ros__parameters: + update_frequency: 1.0 + base_frame_id: base_link + map_frame_id: map + plugins: [static_layer, dynamic_layer, inflation_layer] + inflation_layer: + inflation_radius: 0.55 + robot_radius: 0.22 + static_layer: + map_topic: map + dynamic_layer: + scan_topic: scan + global_frame: map + min_laser_range: 0.0 + max_laser_range: 5.0 + +navyu_global_planner_node: + ros__parameters: + map_frame: map + base_frame: base_link + displacement_threshold: 5.0 + lethal_cost_threshold: 30.0 + +navyu_path_tracker_node: + ros__parameters: + map_frame: map + base_frame: base_link + update_frequency: 100.0 + limit_v_speed: 0.5 + limit_w_speed: 1.0 + yaw_tolerance: 0.07 + gain: 0.1 # to update look_ahead_distance from velocity + look_ahead_const: 0.3 + +navyu_safety_limiter_node: + ros__parameters: + base_frame: base_link + slowdown_ratio: 0.45 + collision_points_threshold_in_polygon: 6 + scan_timeout: 1.0 + robot_radius: 0.5 + length: 0.5 + scale: 1.5 + slowdown_polygon: [-0.4, 0.5, -0.4, -0.5, 0.7, -0.5, 0.7, 0.5] + stop_polygon: [-0.3, 0.3, -0.3, -0.3, 0.5, -0.3, 0.5, 0.3] diff --git a/navyu_navigation/launch/navyu_bringup.launch.py b/navyu_navigation/launch/navyu_bringup.launch.py index 7e3afa4..4c94a1e 100644 --- a/navyu_navigation/launch/navyu_bringup.launch.py +++ b/navyu_navigation/launch/navyu_bringup.launch.py @@ -30,20 +30,8 @@ def generate_launch_description(): map_path = PathJoinSubstitution([FindPackageShare("navyu_navigation"), "map", "map.yaml"]) rviz_config = PathJoinSubstitution([FindPackageShare("navyu_navigation"), "rviz", "navyu.rviz"]) - costmap_map_config_path = PathJoinSubstitution( - [FindPackageShare("navyu_costmap_2d"), "config", "navyu_costmap_2d_params.yaml"] - ) - - navyu_global_planner_config = PathJoinSubstitution( - [FindPackageShare("navyu_path_planner"), "config", "navyu_global_planner_params.yaml"] - ) - - navyu_path_tracker_config = PathJoinSubstitution( - [FindPackageShare("navyu_path_tracker"), "config", "navyu_path_tracker_params.yaml"] - ) - - navyu_safety_limiter_config = PathJoinSubstitution( - [FindPackageShare("navyu_safety_limiter"), "config", "navyu_safety_limiter_params.yaml"] + navyu_config_path = PathJoinSubstitution( + [FindPackageShare("navyu_navigation"), "config", "navyu_params.yaml"] ) lifecycle_node_list = ["map_server"] @@ -63,28 +51,28 @@ def generate_launch_description(): Node( package="navyu_costmap_2d", executable="navyu_costmap_2d_node", - name="global_costmap_node", - parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}], + name="navyu_global_costmap_node", + parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], ), Node( package="navyu_path_planner", executable="navyu_global_planner_node", name="navyu_global_planner_node", - parameters=[navyu_global_planner_config, {"use_sim_time": use_sim_time}], + parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], ), Node( package="navyu_path_tracker", executable="navyu_path_tracker_node", name="navyu_path_tracker_node", remappings=[("/cmd_vel", "/cmd_vel_in")], - parameters=[navyu_path_tracker_config, {"use_sim_time": use_sim_time}], + parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], ), Node( package="navyu_safety_limiter", executable="navyu_safety_limiter_node", - name="safety_limiter_node", + name="navyu_safety_limiter_node", remappings=[("/cmd_vel_out", "/cmd_vel")], - parameters=[navyu_safety_limiter_config, {"use_sim_time": use_sim_time}], + parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], ), Node( package="nav2_lifecycle_manager",