Added front and rear camera ar tag filters to launch

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

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)"/>

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