diff --git a/config/adc_common/landmarks_loc/amcl_mapping.yaml b/config/adc_common/landmarks_loc/amcl_mapping.yaml
index e9e17b8dd228e8df0c494982e1c8eb36f6c0829f..df6f8353ef348c85b6963ddde759652bb5f09fd8 100644
--- a/config/adc_common/landmarks_loc/amcl_mapping.yaml
+++ b/config/adc_common/landmarks_loc/amcl_mapping.yaml
@@ -1,8 +1,9 @@
-rate: 50.0
+rate: 20.0
global_frame: "map"
odom_frame: "adc_car/odom"
base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
amcl_pose_estimated_sigma: 0.25
publish_pose_rate: 0.001
publish_pose_distance: 0.1
diff --git a/config/adc_common/landmarks_loc/landmarks_calibration.yaml b/config/adc_common/landmarks_loc/landmarks_calibration.yaml
index 74476b2c4b683b67a34d7fcd02472648051c6764..f726fedb2f30a1b7736b074fb1bb0b97ff1a8568 100644
--- a/config/adc_common/landmarks_loc/landmarks_calibration.yaml
+++ b/config/adc_common/landmarks_loc/landmarks_calibration.yaml
@@ -1,8 +1,9 @@
-rate: 50.0
+rate: 20.0
global_frame: "map"
odom_frame: "adc_car/odom"
base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
amcl_pose_estimated_sigma: 0.25
publish_pose_rate: 0.001
publish_pose_distance: 0.1
diff --git a/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml b/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml
index 4689ca28f7cc67d44e7aada1e21916584d32f30f..02cc8d13a01eccfd9659efa2a2cfab2e46b40c8b 100644
--- a/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml
+++ b/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml
@@ -1,8 +1,9 @@
-rate: 50.0
+rate: 20.0
global_frame: "map"
odom_frame: "adc_car/odom"
base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
amcl_pose_estimated_sigma: 0.25
publish_pose_rate: 0.001
publish_pose_distance: 0.1
diff --git a/config/adc_common/landmarks_loc/localization.yaml b/config/adc_common/landmarks_loc/localization.yaml
index 076596456b826342915dd0803decad8ed35f7fd0..577d40aae92d710e5fcabd933661ade181c51007 100644
--- a/config/adc_common/landmarks_loc/localization.yaml
+++ b/config/adc_common/landmarks_loc/localization.yaml
@@ -1,8 +1,9 @@
-rate: 50.0
+rate: 20.0
global_frame: "map"
odom_frame: "adc_car/odom"
base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
amcl_pose_estimated_sigma: 0.25
publish_pose_rate: 0.001
publish_pose_distance: 0.1
diff --git a/config/adc_common/landmarks_loc/slam_mapping.yaml b/config/adc_common/landmarks_loc/slam_mapping.yaml
index 6267048f990a2f3561e71085ca678918783d98b7..60b8e3662962a146fe195167b21e3ee73224b1a1 100644
--- a/config/adc_common/landmarks_loc/slam_mapping.yaml
+++ b/config/adc_common/landmarks_loc/slam_mapping.yaml
@@ -1,8 +1,9 @@
-rate: 50.0
+rate: 20.0
global_frame: "map"
odom_frame: "adc_car/odom"
base_link_frame: "adc_car/base_link"
-tf_timeout: 0.1
+tf_timeout: 0.2
+old_feature_timeout: 0.5
amcl_pose_estimated_sigma: 0.25
publish_pose_rate: 0.001
publish_pose_distance: 0.1
diff --git a/launch/adc_global_localization.launch b/launch/adc_global_localization.launch
index 27f1bf73d6f07e5cd78821cc07520af36510fe23..a1bdd509faf9ad01209a8c16e91ecb5b85a6eaa8 100644
--- a/launch/adc_global_localization.launch
+++ b/launch/adc_global_localization.launch
@@ -25,19 +25,8 @@
<arg name="landmarks_config_file" default="amcl_mapping.yaml"/>
<arg name="landmarks_data_dir" default="$(find iri_adc_launch)/data/sample_parking"/>
<arg name="landmarks_file" default="landmarks.txt"/>
- <arg name="features_topic_name" default="/$(arg car_name)/features"/>
<arg name="use_rear_camera_loc" default="false"/>
- <!-- opendrive and map tf -->
- <arg name="opendrive_x" default="0.0"/>
- <arg name="opendrive_y" default="0.0"/>
- <arg name="opendrive_yaw" default="0.0"/>
-
- <node name="static_tf_map_to_opendrive" pkg="tf" type="static_transform_publisher"
- args="$(arg opendrive_x) $(arg opendrive_y) 0 $(arg opendrive_yaw) 0 0 map opendrive 100">
- </node>
-
-
<group ns="$(arg car_name)">
<!-- Front image ROI -->
<include file="$(find iri_image_roi)/launch/node.launch">
@@ -85,7 +74,7 @@
<arg name="config_file" value="$(find iri_adc_launch)/config/adc_common/ar_tag_loc_filter.yaml"/>
<arg name="robot_name" value="$(arg car_name)"/>
<arg name="ar_input_topic" value="/$(arg car_name)/front_ar_pose_marker"/>
- <arg name="features_output_topic" value="$(arg features_topic_name)"/>
+ <arg name="features_output_topic" value="/$(arg car_name)/front_features"/>
</include>
@@ -136,7 +125,7 @@
<arg name="config_file" value="$(find iri_adc_launch)/config/adc_common/ar_tag_loc_filter.yaml"/>
<arg name="robot_name" value="$(arg car_name)"/>
<arg name="ar_input_topic" value="/$(arg car_name)/rear_ar_pose_marker"/>
- <arg name="features_output_topic" value="$(arg features_topic_name)"/>
+ <arg name="features_output_topic" value="/$(arg car_name)/rear_features"/>
</include>
</group>
@@ -148,45 +137,14 @@
<arg name="landmarks_map_file" value="$(arg landmarks_data_dir)/$(arg landmarks_file)"/>
<arg name="estimated_pose_topic_name" value="/$(arg car_name)/estimated_pose"/>
<arg name="initialpose_topic_name" value="/$(arg car_name)/initialpose"/>
- <arg name="features_topic_name" value="$(arg features_topic_name)"/>
+ <arg name="front_features_topic_name" value="/$(arg car_name)/front_features"/>
+ <arg name="rear_features_topic_name" value="/$(arg car_name)/rear_features"/>
<arg name="initial_pose_x" value="$(arg car_x)"/>
<arg name="initial_pose_y" value="$(arg car_y)"/>
<arg name="initial_pose_yaw" value="$(arg car_yaw)"/>
</include>
</group>
- <include file="$(find iri_rosnav)/launch/include/map_server.launch">
- <arg name="ns" value="$(arg car_name)"/>
- <arg name="map_frame_id" value="map"/>
- <arg name="map_path" value="$(arg map_path)"/>
- <arg name="map_name" value="$(arg map_name)"/>
- <arg name="map_topic" value="/$(arg car_name)/map"/>
- <arg name="map_service" value="/$(arg car_name)/static_map"/>
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- </include>
-
- <include file="$(find iri_rosnav)/launch/include/amcl.launch">
- <arg name="ns" value="$(arg car_name)"/>
- <arg name="config" value="$(arg amcl_config)"/>
- <arg name="map_frame_id" value="map"/>
- <arg name="odom_frame_id" value="$(arg car_name)/odom"/>
- <arg name="base_frame_id" value="$(arg car_name)/base_footprint"/>
- <arg name="initial_x" value="$(arg car_x)"/>
- <arg name="initial_y" value="$(arg car_y)"/>
- <arg name="initial_yaw" value="$(arg car_yaw)"/>
- <arg name="scan_topic" value="/$(arg car_name)/sensors/scan"/>
- <arg name="map_service" value="/$(arg car_name)/static_map"/>
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- </include>
-
- <include file="$(find iri_model_car_rosnav)/launch/include/localization.launch">
- <arg name="name" value="$(arg car_name)"/>
- <arg name="global_ekf_config_file" value="$(arg global_ekf_config_file)"/>
- <arg name="set_pose_topic" value="/$(arg car_name)/initialpose"/>
- </include>
-
<node name="rqt_reconfigure_iri_adc_tag_localization_filter"
pkg ="rqt_reconfigure"
type="rqt_reconfigure"