From a6538956925de191da60d282a6ef573440c92a2c Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 15 Mar 2021 18:34:36 +0100
Subject: [PATCH] Updated the configuration parameters. Grouped all the map
 related files in a folder with the name of the map.

---
 config/adc_common/adc_odometry_config.yaml             |  4 ++--
 config/adc_rosnav/costmap/common_params.yaml           |  2 +-
 .../global_planner/iri_opendrive_params.yaml           |  2 +-
 config/adc_rosnav/local_planner/ackermann_params.yaml  |  6 +++---
 config/adc_sim/adc_controller_config.yaml              |  8 ++++----
 data/{ => sample}/sample.xodr                          |  1 -
 launch/adc_bringup.launch                              | 10 +++++-----
 launch/adc_navigation.launch                           |  7 ++-----
 launch/adc_sim_env.launch                              |  4 ++--
 maps/{adc_map.pgm => sample/sample.pgm}                |  0
 maps/{adc_map.yaml => sample/sample.yaml}              |  0
 11 files changed, 20 insertions(+), 24 deletions(-)
 rename data/{ => sample}/sample.xodr (99%)
 rename maps/{adc_map.pgm => sample/sample.pgm} (100%)
 rename maps/{adc_map.yaml => sample/sample.yaml} (100%)

diff --git a/config/adc_common/adc_odometry_config.yaml b/config/adc_common/adc_odometry_config.yaml
index 7aefdcc..0469bf1 100644
--- a/config/adc_common/adc_odometry_config.yaml
+++ b/config/adc_common/adc_odometry_config.yaml
@@ -7,8 +7,8 @@ wheel_distance: 0.216
 axel_distance: 0.3662
 filter_coeff: 0.3
 speed_deadband: 0.1
-max_steer_angle: 0.41
-min_steer_angle: -0.41
+max_steer_angle: 0.4
+min_steer_angle: -0.4
 max_steer_control: 75.0
 min_steer_control: -68.0
 publish_tf: False
diff --git a/config/adc_rosnav/costmap/common_params.yaml b/config/adc_rosnav/costmap/common_params.yaml
index f795dd3..9d3714b 100644
--- a/config/adc_rosnav/costmap/common_params.yaml
+++ b/config/adc_rosnav/costmap/common_params.yaml
@@ -42,4 +42,4 @@ inflation_layer:
   enabled: true
   inflate_unknown: false
   cost_scaling_factor:  2.0
-  inflation_radius:     1.0
+  inflation_radius:     0.1
diff --git a/config/adc_rosnav/global_planner/iri_opendrive_params.yaml b/config/adc_rosnav/global_planner/iri_opendrive_params.yaml
index 07238a3..6aaa42b 100644
--- a/config/adc_rosnav/global_planner/iri_opendrive_params.yaml
+++ b/config/adc_rosnav/global_planner/iri_opendrive_params.yaml
@@ -4,7 +4,7 @@ OpendriveGlobalPlanner:
   dist_tol: 0.3
   multi_hyp: True
   resolution: 0.01
-  scale_factor: 8.0
+  scale_factor: 10.0
   min_road_length: 0.01
   cost_type: 0
 
diff --git a/config/adc_rosnav/local_planner/ackermann_params.yaml b/config/adc_rosnav/local_planner/ackermann_params.yaml
index ca377f4..5fb14b1 100644
--- a/config/adc_rosnav/local_planner/ackermann_params.yaml
+++ b/config/adc_rosnav/local_planner/ackermann_params.yaml
@@ -31,10 +31,10 @@ AckermannPlannerROS:
   restore_defaults: false
 
   max_trans_vel: 1.0
-  min_trans_vel: -1.0
+  min_trans_vel: 0.11
   max_trans_acc: 0.2
-  max_steer_angle: 0.42
-  min_steer_angle: -0.42
+  max_steer_angle: 0.40
+  min_steer_angle: -0.40
   max_steer_vel: 0.3
   min_steer_vel: -0.3
   max_steer_acc: 1.0
diff --git a/config/adc_sim/adc_controller_config.yaml b/config/adc_sim/adc_controller_config.yaml
index 8ed18e4..7caa994 100644
--- a/config/adc_sim/adc_controller_config.yaml
+++ b/config/adc_sim/adc_controller_config.yaml
@@ -19,12 +19,12 @@
     encoder_rate: 40
     min_steer_angle: -0.4
     max_steer_angle: 0.4
-    min_steer_control: -100.0
-    max_steer_control: 100.0
+    min_steer_control: -68.0
+    max_steer_control: 75.0
     min_speed: -1.5
     max_speed: 1.5
-    min_speed_control: -77.0
-    max_speed_control: 62.0
+    min_speed_control: -100.0
+    max_speed_control: 100.0
     control_topic: /adc_car/control
     encoders_topic: /adc_car/encoders
     
diff --git a/data/sample.xodr b/data/sample/sample.xodr
similarity index 99%
rename from data/sample.xodr
rename to data/sample/sample.xodr
index e6a0a8b..a7d7005 100644
--- a/data/sample.xodr
+++ b/data/sample/sample.xodr
@@ -40,7 +40,6 @@
                 </right>
             </laneSection>
         </lanes>
-        <objects />
         <signals>
             <signal s="1.0000000000000000e+01" t="-5.0000000000000000e+00" id="1001" name="pedestrian0_1" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="350" subtype="-1" value="0.0000000000000000e+00" />
             <signal s="1.5000000000000000e+01" t="5.0000000000000000e+00" id="1002" name="pedestrian0_2" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="350" subtype="-1" value="0.0000000000000000e+00" />
diff --git a/launch/adc_bringup.launch b/launch/adc_bringup.launch
index 40b3139..e27259d 100644
--- a/launch/adc_bringup.launch
+++ b/launch/adc_bringup.launch
@@ -14,10 +14,10 @@
     <arg name="delock_format_file"         value="$(find iri_adc_launch)/config/adc/delock_camera_config.yaml" />
     <arg name="delock_calib_file"          value="$(find iri_adc_launch)/calibration/delock_1280x960.yaml" />
     <arg name="rplidar_config_file"        value="$(find iri_adc_launch)/config/adc/rplidar_config.yaml" />
-   <arg name="control_config_file"         value="$(find iri_adc_launch)/config/adc_common/adc_control_config.yaml"/>
-   <arg name="odometry_config_file"        value="$(find iri_adc_launch)/config/adc_common/adc_odometry_config.yaml"/>
-   <arg name="cmd_vel_mux_config_file"     value="$(find iri_adc_launch)/config/adc_common/mux.yaml"/>
-   <arg name="local_ekf_config_file"       value="$(find iri_adc_launch)/config/adc_common/adc_local_ekf_odom_imu.yaml"/>
-   <arg name="sim_config_path"             value="$(find iri_adc_launch)/config/adc_sim"/>
+    <arg name="control_config_file"         value="$(find iri_adc_launch)/config/adc_common/adc_control_config.yaml"/>
+    <arg name="odometry_config_file"        value="$(find iri_adc_launch)/config/adc_common/adc_odometry_config.yaml"/>
+    <arg name="cmd_vel_mux_config_file"     value="$(find iri_adc_launch)/config/adc_common/mux.yaml"/>
+    <arg name="local_ekf_config_file"       value="$(find iri_adc_launch)/config/adc_common/adc_local_ekf_odom_imu.yaml"/>
+    <arg name="sim_config_path"             value="$(find iri_adc_launch)/config/adc_sim"/>
   </include>
 </launch>
diff --git a/launch/adc_navigation.launch b/launch/adc_navigation.launch
index 58f18fa..c5407d6 100644
--- a/launch/adc_navigation.launch
+++ b/launch/adc_navigation.launch
@@ -10,14 +10,11 @@
   <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="opendrive_path"          default="$(find iri_adc_launch)/data"/>
   <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="car_x"                       default="0.0"/>
   <arg name="car_y"                       default="0.0"/>
   <arg name="car_yaw"                     default="0.0"/>
@@ -60,6 +57,6 @@
     <arg name="set_pose_topic"          value="/$(arg name)/initialpose"/>
   </include>
 
-  <param name="/$(arg name)/move_base/OpendriveGlobalPlanner/opendrive_file" value="$(arg opendrive_file_path)/$(arg opendrive_file_name).xodr" />
+  <param name="/$(arg name)/move_base/OpendriveGlobalPlanner/opendrive_file" value="$(arg opendrive_path)/$(arg map_name).xodr" />
 
 </launch>
diff --git a/launch/adc_sim_env.launch b/launch/adc_sim_env.launch
index d714843..578ddde 100644
--- a/launch/adc_sim_env.launch
+++ b/launch/adc_sim_env.launch
@@ -34,10 +34,10 @@
     <arg name="local_ekf_config_file"      value="$(find iri_adc_launch)/config/adc_common/adc_local_ekf_odom_imu.yaml"/>
   </include>
 
-  <include file="$(find iri_adc_launch)/launch/sample_signals.launch">
+  <include file="$(find iri_adc_launch)/launch/$(arg road_name)/$(arg road_name)_signals.launch">
   </include>
 
-  <include file="$(find iri_adc_launch)/launch/sample_objects.launch">
+  <include file="$(find iri_adc_launch)/launch/$(arg road_name)/$(arg road_name)_objects.launch">
   </include>
 
   <include file="$(find iri_road_description)/launch/spawn_road.launch">
diff --git a/maps/adc_map.pgm b/maps/sample/sample.pgm
similarity index 100%
rename from maps/adc_map.pgm
rename to maps/sample/sample.pgm
diff --git a/maps/adc_map.yaml b/maps/sample/sample.yaml
similarity index 100%
rename from maps/adc_map.yaml
rename to maps/sample/sample.yaml
-- 
GitLab