diff --git a/config/adc_common/ar_tag_loc_filter.yaml b/config/adc_common/ar_tag_loc_filter.yaml
index 766496d23a45fcaf334c1581bc1f1bb4aa104002..06cb6596eab12cb98a9b4a3a030af37e619e77cb 100644
--- a/config/adc_common/ar_tag_loc_filter.yaml
+++ b/config/adc_common/ar_tag_loc_filter.yaml
@@ -12,7 +12,7 @@ ids_enabled: "0, 1, 2"
time_persistance_filter_en: True
time_persistance_filter_orientation_en: True
-time_persistance_alpha_window: 0.1
+time_persistance_alpha_window: 0.15
time_persistance_range_window: 0.2
time_persistance_orientation_th: 0.4
time_persistance_min_detections: 4
diff --git a/launch/adc_global_localization.launch b/launch/adc_global_localization.launch
index cfb44179e6e95a67f8cb9b5a83d9281e3faeab43..ecc72282439f165c681405cc54c8cee1587163d5 100644
--- a/launch/adc_global_localization.launch
+++ b/launch/adc_global_localization.launch
@@ -20,10 +20,13 @@
<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"/>
+
- <!-- Ar track alvar (bringup??) -->
<group ns="$(arg car_name)">
- <node name="ar_track_alvar"
+ <!--Front ar track_alvar -->
+ <node name="front_ar_track_alvar"
pkg="ar_track_alvar"
type="individualMarkersNoKinect"
respawn="false"
@@ -37,20 +40,23 @@
<remap from="/$(arg car_name)/camera_info" to="/$(arg car_name)/sensors/basler_camera_cropped/camera_info" />-->
<remap from="/$(arg car_name)/camera_image" to="/$(arg car_name)/sensors/basler_camera/inverted_image_raw" />
<remap from="/$(arg car_name)/camera_info" to="/$(arg car_name)/sensors/basler_camera/camera_info" />
- <remap from="/$(arg car_name)/ar_pose_marker" to="/$(arg car_name)/ar_pose_marker" />
+ <remap from="/$(arg car_name)/ar_pose_marker" to="/$(arg car_name)/front_ar_pose_marker" />
</node>
+ <!-- Front ar tag filter -->
<include file="$(find iri_adc_tag_localization_filter)/launch/node.launch">
- <arg name="node_name" value="iri_adc_tag_localization_filter"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <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)/ar_pose_marker"/>
+ <arg name="node_name" value="front_iri_adc_tag_localization_filter"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <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)"/>
</include>
+ <!-- Front image inverter -->
<include file="$(find iri_image_inverter)/launch/node.launch">
- <arg name="node_name" value="iri_image_inverter"/>
+ <arg name="node_name" value="front_iri_image_inverter"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<!--<arg name="image_in_topic" value="/$(arg car_name)/sensors/basler_camera_cropped/image_raw"/>
@@ -59,6 +65,7 @@
<arg name="image_out_topic" value="/$(arg car_name)/sensors/basler_camera/inverted_image_raw"/>
</include>
+
<!--<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>
<node name="crop_image"
@@ -70,6 +77,48 @@
<rosparam file="$(find iri_adc_launch)/config/adc_common/crop_front_image.yaml" command="load"/>
</node>-->
+ <group if="$(arg use_rear_camera_loc)">
+ <!--rear ar track_alvar -->
+ <node name="rear_ar_track_alvar"
+ pkg="ar_track_alvar"
+ type="individualMarkersNoKinect"
+ respawn="false"
+ output="screen">
+ <param name="marker_size" type="double" value="$(arg marker_size)" />
+ <param name="max_new_marker_error" type="double" value="0.08" />
+ <param name="max_track_error" type="double" value="0.2" />
+ <param name="marker_margin" type="double" value="1.0" />
+ <param name="output_frame" type="string" value="$(arg car_name)/rear_camera_uvc_camera_optical" />
+ <!--<remap from="/$(arg car_name)/camera_image" to="/$(arg car_name)/sensors/delock_camera_cropped/inverted_image_raw" />
+ <remap from="/$(arg car_name)/camera_info" to="/$(arg car_name)/sensors/delock_camera_cropped/camera_info" />-->
+ <remap from="/$(arg car_name)/camera_image" to="/$(arg car_name)/sensors/delock_camera/inverted_image_raw" />
+ <remap from="/$(arg car_name)/camera_info" to="/$(arg car_name)/sensors/delock_camera/camera_info" />
+ <remap from="/$(arg car_name)/ar_pose_marker" to="/$(arg car_name)/rear_ar_pose_marker" />
+ </node>
+
+ <!-- Front ar tag filter -->
+ <include file="$(find iri_adc_tag_localization_filter)/launch/node.launch">
+ <arg name="node_name" value="rear_iri_adc_tag_localization_filter"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <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)"/>
+ </include>
+
+ <!-- Front image inverter -->
+ <include file="$(find iri_image_inverter)/launch/node.launch">
+ <arg name="node_name" value="rear_iri_image_inverter"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <!--<arg name="image_in_topic" value="/$(arg car_name)/sensors/delock_camera_cropped/image_raw"/>
+ <arg name="image_out_topic" value="/$(arg car_name)/sensors/delock_camera_cropped/inverted_image_raw"/>-->
+ <arg name="image_in_topic" value="/$(arg car_name)/sensors/delock_camera/image_raw"/>
+ <arg name="image_out_topic" value="/$(arg car_name)/sensors/delock_camera/inverted_image_raw"/>
+ </include>
+ </group>
+
<include file="$(find iri_adc_landmarks_slam_solver)/launch/node.launch">
<arg name="node_name" value="iri_adc_landmarks_slam_solver"/>
<arg name="output" value="$(arg output)"/>
@@ -78,7 +127,7 @@
<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 car_name)/iri_adc_tag_localization_filter/features"/>
+ <arg name="features_topic_name" value="$(arg features_topic_name)"/>
<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)"/>
diff --git a/rviz/localization_global.rviz b/rviz/localization_global.rviz
index 79530424db206a654fb3dc06ee90057cdd652d7a..13b8fd3960387f09d359415600ea8674ef678325 100644
--- a/rviz/localization_global.rviz
+++ b/rviz/localization_global.rviz
@@ -747,10 +747,18 @@ Visualization Manager:
Value: true
- Class: rviz/MarkerArray
Enabled: true
- Marker Topic: /adc_car/iri_adc_tag_localization_filter/vis_features
- Name: Features
+ Marker Topic: /adc_car/front_iri_adc_tag_localization_filter/vis_features
+ Name: FrontFeatures
Namespaces:
- adc_car/front_camera_uvc_camera_optical_features: true
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /adc_car/rear_iri_adc_tag_localization_filter/vis_features
+ Name: RearFeatures
+ Namespaces:
+ {}
Queue Size: 100
Value: true
- Class: rviz/MarkerArray