diff --git a/navigation/path_planning/config/nav2_params_real.yaml b/navigation/path_planning/config/nav2_params_real.yaml index eafe53f..0c4140e 100644 --- a/navigation/path_planning/config/nav2_params_real.yaml +++ b/navigation/path_planning/config/nav2_params_real.yaml @@ -108,7 +108,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 0.8 - inflation_radius: 0.3 + inflation_radius: 0.3 always_send_full_costmap: True local_costmap_client: @@ -140,7 +140,7 @@ bt_navigator: use_sim_time: False global_frame: map robot_base_frame: base_link - odom_topic: /odom_with_offset + odom_topic: /lio_sam/mapping/odometry bt_loop_duration: 10 default_server_timeout: 20 enable_groot_monitoring: True @@ -233,7 +233,7 @@ controller_server: ## Server parameters use_sim_time: False controller_frequency: 1.0 - odom_topic: "odom_with_offset" + odom_topic: "lio_sam/mapping/odometry" # Min velocities to ignore min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 diff --git a/navigation/path_planning/config/offset.yaml b/navigation/path_planning/config/offset.yaml index 084d6aa..ebcc71a 100644 --- a/navigation/path_planning/config/offset.yaml +++ b/navigation/path_planning/config/offset.yaml @@ -1,10 +1,10 @@ position_offset: - - 17.228 # was 11 - - -11.549 # was 17 + - 0.0 # was 11 + - 0.0 # was 17 - 0.0 orientation_offset: - 0.0 - 0.0 - - 0.7071 - - 0.7071 + - 0.0 + - 0.0 diff --git a/navigation/path_planning/launch/nav2_real.launch.py b/navigation/path_planning/launch/nav2_real.launch.py index 5e6143c..bbbd2ac 100644 --- a/navigation/path_planning/launch/nav2_real.launch.py +++ b/navigation/path_planning/launch/nav2_real.launch.py @@ -59,6 +59,9 @@ def launch_setup(context: launch.LaunchContext, *args, **kwargs): {"use_sim_time": False}, ], ) + + + global_robot_localization_node = launch_ros.actions.Node( package="robot_localization", @@ -81,11 +84,13 @@ def launch_setup(context: launch.LaunchContext, *args, **kwargs): return [ # Commands start_nav2_cmd, - start_wheels_control_cmd, + # start_wheels_control_cmd, # Nodes # local_robot_localization_node, # global_robot_localization_node, # odom_offset_node + map, + ]