From 60aac4472ed4057b762de2f25aaa6f4bcb3a3ad4 Mon Sep 17 00:00:00 2001 From: Alejandro Lopez Gestoso <alopez@iri.upc.edu> Date: Wed, 10 Mar 2021 13:56:52 +0100 Subject: [PATCH] Adapted to share navigation launch and config files --- .../adc_global_ekf_odom_imu_amcl.yaml} | 22 ++++++++-------- config/adc_rosnav/amcl_no_tf.yaml | 4 +-- .../adc_rosnav/local_planner/dwa_params.yaml | 2 +- ...avigation.launch => adc_navigation.launch} | 25 ++++++++++++------- maps/adc_map.yaml | 1 + 5 files changed, 32 insertions(+), 22 deletions(-) rename config/{adc_car_global_ekf.yaml => adc_common/adc_global_ekf_odom_imu_amcl.yaml} (77%) rename launch/{adc_sim_navigation.launch => adc_navigation.launch} (74%) diff --git a/config/adc_car_global_ekf.yaml b/config/adc_common/adc_global_ekf_odom_imu_amcl.yaml similarity index 77% rename from config/adc_car_global_ekf.yaml rename to config/adc_common/adc_global_ekf_odom_imu_amcl.yaml index c6b8455..5a0b325 100644 --- a/config/adc_car_global_ekf.yaml +++ b/config/adc_common/adc_global_ekf_odom_imu_amcl.yaml @@ -5,7 +5,9 @@ two_d_mode: true print_diagnostics: true debug: false debug_out_file: /tmp/debug_ekf_localization.txt -use_control: false +use_control: true +control_config: [true, true, false, false, false, true] + publish_tf: true map_frame: map @@ -16,19 +18,19 @@ world_frame: map odom0: /adc_car/odom odom0_config: [false, false, false, false, false, false, - true, true, false, + true, false, false, false, false, true, false, false, false] -#imu0: /adc_car/sensors/imu_enu -#imu0_config: [false, false, false, -# false, false, false, -# false, false, false, -# false, false, true, -# true, false, false] +imu0: /adc_car/sensors/imu_data +imu0_config: [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] -#imu0_differential: false -#imu0_relative: true +imu0_differential: false +imu0_relative: true #imu0_remove_gravitational_acceleration: true pose0: /adc_car/amcl_pose diff --git a/config/adc_rosnav/amcl_no_tf.yaml b/config/adc_rosnav/amcl_no_tf.yaml index 98d6e7c..af186af 100644 --- a/config/adc_rosnav/amcl_no_tf.yaml +++ b/config/adc_rosnav/amcl_no_tf.yaml @@ -1,7 +1,7 @@ #frames defined in amc.launch #map_frame_id: map -#odom_frame_id: model_car/odom -#base_frame_id: model_car/base_footprint +#odom_frame_id: adc_car/odom +#base_frame_id: adc_car/base_footprint initial_pose_x: 0.0 initial_pose_y: 0.0 initial_pose_a: 0.0 diff --git a/config/adc_rosnav/local_planner/dwa_params.yaml b/config/adc_rosnav/local_planner/dwa_params.yaml index c9d22a1..0def2b5 100644 --- a/config/adc_rosnav/local_planner/dwa_params.yaml +++ b/config/adc_rosnav/local_planner/dwa_params.yaml @@ -50,5 +50,5 @@ DWAPlannerROS: #not in dynamic reconfigure publish_traj_pc: false - global_frame_id: model_car/odom + global_frame_id: adc_car/odom publish_cost_grid_pc: false diff --git a/launch/adc_sim_navigation.launch b/launch/adc_navigation.launch similarity index 74% rename from launch/adc_sim_navigation.launch rename to launch/adc_navigation.launch index 8a69d7c..58f18fa 100644 --- a/launch/adc_sim_navigation.launch +++ b/launch/adc_navigation.launch @@ -2,22 +2,29 @@ <!-- --> <launch> - <arg name="name" default="adc_car"/> - <arg name="output" default="screen"/> - <arg name="launch_prefix" default=""/> - <arg name="map_path" default="$(find iri_adc_launch)/maps"/> - <arg name="map_name" default="adc_map" /> + <arg name="name" default="adc_car"/> + <arg name="output" default="screen" /> + <arg name="launch_prefix" default="" /> + <arg name="move_base_params" default="move_base_params.yaml"/> + <arg name="costmap_common_params" default="common_params.yaml"/> + <arg name="costmap_local_params" default="local_params.yaml"/> + <arg name="costmap_global_params" default="global_params.yaml"/> + <arg name="map_path" default="$(find iri_adc_launch)/maps"/> + <arg name="map_name" default="adc_map" /> + + <arg name="global_ekf_config_file" default="$(find iri_adc_launch)/config/adc_common/adc_global_ekf_odom_imu_amcl.yaml"/> + + <arg name="opendrive_file_path" default="$(find iri_adc_launch)/data"/> <arg name="opendrive_file_name" default="sample"/> - <arg name="gazebo_gui" default="True"/> <arg name="car_x" default="0.0"/> <arg name="car_y" default="0.0"/> <arg name="car_yaw" default="0.0"/> <include file="$(find iri_rosnav)/launch/nav.launch"> <arg name="ns" value="$(arg name)"/> - <arg name="path" value="$(find iri_adc_launch)/config/$(arg name)_move_base"/> + <arg name="path" value="$(find iri_adc_launch)/config/adc_rosnav"/> <arg name="move_base_params" value="move_base_params.yaml"/> <arg name="costmap_common_params" value="common_params.yaml"/> <arg name="costmap_local_params" value="local_params.yaml"/> @@ -35,7 +42,7 @@ <arg name="map_path" value="$(arg map_path)"/> <arg name="map_name" value="$(arg map_name)"/> <arg name="use_amcl" value="true"/> - <arg name="amcl_config" value="$(find iri_model_car_rosnav)/params/amcl_no_tf.yaml"/> + <arg name="amcl_config" value="$(find iri_adc_launch)/config/adc_rosnav/amcl_no_tf.yaml"/> <arg name="initial_x" value="$(arg car_x)"/> <arg name="initial_y" value="$(arg car_y)"/> <arg name="initial_yaw" value="$(arg car_yaw)"/> @@ -49,7 +56,7 @@ <include file="$(find iri_model_car_rosnav)/launch/include/localization.launch"> <arg name="name" value="$(arg name)"/> - <arg name="global_ekf_config_file" value="$(find iri_adc_launch)/config/adc_car_global_ekf.yaml"/> + <arg name="global_ekf_config_file" value="$(arg global_ekf_config_file)"/> <arg name="set_pose_topic" value="/$(arg name)/initialpose"/> </include> diff --git a/maps/adc_map.yaml b/maps/adc_map.yaml index 259c055..cf92b8e 100644 --- a/maps/adc_map.yaml +++ b/maps/adc_map.yaml @@ -4,3 +4,4 @@ origin: [-5.000000, -5.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 + -- GitLab