From 57b6f2a8e8b00c6fadb0a869cd2a9c28a0ad4c7b Mon Sep 17 00:00:00 2001
From: Yukihiro Saito <yukky.saito@gmail.com>
Date: Thu, 16 Jul 2020 19:18:51 +0900
Subject: [PATCH] Modify to change calib param for each vehicle (#51)

* add vehicle id to change parameter of each vehicle

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* add vehicle id to change param for each vehicle

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>
---
 .../{ => default}/aip_customized/sensors_calibration.yaml   | 0
 .../config/{ => default}/aip_s1/sensors_calibration.yaml    | 0
 .../config/{ => default}/aip_x1/sensors_calibration.yaml    | 0
 .../config/{ => default}/aip_x2/sensors_calibration.yaml    | 0
 .../{ => default}/aip_xx1/sensor_kit_calibration.yaml       | 0
 .../config/{ => default}/aip_xx1/sensors_calibration.yaml   | 0
 .../config/{ => default}/aip_xx2/sensors_calibration.yaml   | 0
 launch/vehicle_launch/launch/vehicle.launch                 | 4 ++++
 launch/vehicle_launch/launch/vehicle_description.launch     | 4 +++-
 launch/vehicle_launch/launch/vehicle_interface.launch       | 6 ++++--
 10 files changed, 11 insertions(+), 3 deletions(-)
 rename launch/vehicle_launch/config/{ => default}/aip_customized/sensors_calibration.yaml (100%)
 rename launch/vehicle_launch/config/{ => default}/aip_s1/sensors_calibration.yaml (100%)
 rename launch/vehicle_launch/config/{ => default}/aip_x1/sensors_calibration.yaml (100%)
 rename launch/vehicle_launch/config/{ => default}/aip_x2/sensors_calibration.yaml (100%)
 rename launch/vehicle_launch/config/{ => default}/aip_xx1/sensor_kit_calibration.yaml (100%)
 rename launch/vehicle_launch/config/{ => default}/aip_xx1/sensors_calibration.yaml (100%)
 rename launch/vehicle_launch/config/{ => default}/aip_xx2/sensors_calibration.yaml (100%)

diff --git a/launch/vehicle_launch/config/aip_customized/sensors_calibration.yaml b/launch/vehicle_launch/config/default/aip_customized/sensors_calibration.yaml
similarity index 100%
rename from launch/vehicle_launch/config/aip_customized/sensors_calibration.yaml
rename to launch/vehicle_launch/config/default/aip_customized/sensors_calibration.yaml
diff --git a/launch/vehicle_launch/config/aip_s1/sensors_calibration.yaml b/launch/vehicle_launch/config/default/aip_s1/sensors_calibration.yaml
similarity index 100%
rename from launch/vehicle_launch/config/aip_s1/sensors_calibration.yaml
rename to launch/vehicle_launch/config/default/aip_s1/sensors_calibration.yaml
diff --git a/launch/vehicle_launch/config/aip_x1/sensors_calibration.yaml b/launch/vehicle_launch/config/default/aip_x1/sensors_calibration.yaml
similarity index 100%
rename from launch/vehicle_launch/config/aip_x1/sensors_calibration.yaml
rename to launch/vehicle_launch/config/default/aip_x1/sensors_calibration.yaml
diff --git a/launch/vehicle_launch/config/aip_x2/sensors_calibration.yaml b/launch/vehicle_launch/config/default/aip_x2/sensors_calibration.yaml
similarity index 100%
rename from launch/vehicle_launch/config/aip_x2/sensors_calibration.yaml
rename to launch/vehicle_launch/config/default/aip_x2/sensors_calibration.yaml
diff --git a/launch/vehicle_launch/config/aip_xx1/sensor_kit_calibration.yaml b/launch/vehicle_launch/config/default/aip_xx1/sensor_kit_calibration.yaml
similarity index 100%
rename from launch/vehicle_launch/config/aip_xx1/sensor_kit_calibration.yaml
rename to launch/vehicle_launch/config/default/aip_xx1/sensor_kit_calibration.yaml
diff --git a/launch/vehicle_launch/config/aip_xx1/sensors_calibration.yaml b/launch/vehicle_launch/config/default/aip_xx1/sensors_calibration.yaml
similarity index 100%
rename from launch/vehicle_launch/config/aip_xx1/sensors_calibration.yaml
rename to launch/vehicle_launch/config/default/aip_xx1/sensors_calibration.yaml
diff --git a/launch/vehicle_launch/config/aip_xx2/sensors_calibration.yaml b/launch/vehicle_launch/config/default/aip_xx2/sensors_calibration.yaml
similarity index 100%
rename from launch/vehicle_launch/config/aip_xx2/sensors_calibration.yaml
rename to launch/vehicle_launch/config/default/aip_xx2/sensors_calibration.yaml
diff --git a/launch/vehicle_launch/launch/vehicle.launch b/launch/vehicle_launch/launch/vehicle.launch
index 653c7b5271375..5873b2bf6593a 100644
--- a/launch/vehicle_launch/launch/vehicle.launch
+++ b/launch/vehicle_launch/launch/vehicle.launch
@@ -2,17 +2,21 @@
 <launch>
   <arg name="sensor_model"/>
   <arg name="vehicle_model"/>
+
   <arg name="simulation" default="false" />
+  <arg name ="vehicle_id" value="$(optenv VEHICLE_ID default)" />
 
   <!-- vehicle description -->
   <include file="$(find vehicle_launch)/launch/vehicle_description.launch">
     <arg name="vehicle_model" value="$(arg vehicle_model)"/>
     <arg name="sensor_model" value="$(arg sensor_model)"/>
+    <arg name="vehicle_id" value="$(arg vehicle_id)"/>
   </include>
 
   <!-- vehicle interface -->
   <include file="$(find vehicle_launch)/launch/vehicle_interface.launch">
     <arg name="simulation" value="$(arg simulation)"/>
     <arg name="vehicle_model" value="$(arg vehicle_model)"/>
+    <arg name="vehicle_id" value="$(arg vehicle_id)"/>
   </include>
 </launch>
diff --git a/launch/vehicle_launch/launch/vehicle_description.launch b/launch/vehicle_launch/launch/vehicle_description.launch
index 341a2ae8eed79..7a09cd6e13bb4 100644
--- a/launch/vehicle_launch/launch/vehicle_description.launch
+++ b/launch/vehicle_launch/launch/vehicle_description.launch
@@ -3,9 +3,11 @@
   <arg name="sensor_model"/>
   <arg name="vehicle_model"/>
 
+  <arg name ="vehicle_id" />
+
   <arg name="model" value="$(find vehicle_launch)/urdf/vehicle.xacro"/>
 
-  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model) vehicle_model:=$(arg vehicle_model) sensor_model:=$(arg sensor_model) config_dir:=$(find vehicle_launch)/config/$(arg sensor_model)"/>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model) vehicle_model:=$(arg vehicle_model) sensor_model:=$(arg sensor_model) config_dir:=$(find vehicle_launch)/config/$(arg vehicle_id)/$(arg sensor_model)"/>
 
   <group ns="/vehicle_info">
     <rosparam file="$(eval find(arg('vehicle_model')+'_description')+'/config/vehicle_info.yaml')" command="load"/>
diff --git a/launch/vehicle_launch/launch/vehicle_interface.launch b/launch/vehicle_launch/launch/vehicle_interface.launch
index 16706c1ee9769..d01796fe921e8 100644
--- a/launch/vehicle_launch/launch/vehicle_interface.launch
+++ b/launch/vehicle_launch/launch/vehicle_interface.launch
@@ -2,15 +2,17 @@
 <launch>
   <arg name="simulation" />
   <arg name="vehicle_model" />
-  <arg name="simulator_model" value="$(eval find(arg('vehicle_model')+'_description')+'/config/simulator_model.yaml')" />
+  <arg name="vehicle_id" />
 
   <!-- simulation mode -->
   <group if="$(eval simulation==true)">
+    <arg name="simulator_model" value="$(eval find(arg('vehicle_model')+'_description')+'/config/simulator_model.yaml')" />
     <include file="$(find simple_planning_simulator)/launch/simple_planning_simulator.launch" pass_all_args="true"/>
   </group>
 
   <!-- real mode -->
   <group unless="$(eval simulation==true)">
-    <include file="$(eval find(arg('vehicle_model')+'_description')+'/launch/vehicle_interface.launch')"/>
+    <include file="$(eval find(arg('vehicle_model')+'_description')+'/launch/vehicle_interface.launch')" >
+    </include>
   </group>
 </launch>