Updated to launch and configure iri_adc_landmarks_slam solver

config/adc_common/ar_tag_loc_filter.yaml

+rate: 100
+publish_visualization: True
+
+range_filter_en: True
+detection_max_range: 4.0
+
+confidence_filter_en: False
+detection_min_confidence: 126
+
+id_filter_en: True
+ids_enabled: "0, 1, 2"
+
+time_persistance_filter_en: True
+time_persistance_alpha_window: 0.1
+time_persistance_range_window: 0.2
+time_persistance_min_detections: 4

config/adc_common/landmarks_loc/amcl_mapping.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+publish_visualization : True
+load_landmarks : False
+landmarks_pos_fixed : False
+search_for_new_landmarks : True
+first_robot_state_fixed : False
+last_robot_state_fixed : True
+all_robot_states_fixed : True
+amcl_localization : True
+calculate_covariance : False
+publish_tf_map_odom : False
+
+update_problem_rate: 0.1
+update_problem_distance: 0.2
+update_problem_angle: 0.1
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.15
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/adc_common/landmarks_loc/landmarks_calibration.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+publish_visualization : True
+load_landmarks : True
+landmarks_pos_fixed : False
+search_for_new_landmarks : False
+first_robot_state_fixed : True
+last_robot_state_fixed : False
+all_robot_states_fixed : False
+amcl_localization : False
+calculate_covariance : False
+publish_tf_map_odom : True
+
+update_problem_rate: 0.1
+update_problem_distance: 0.2
+update_problem_angle: 0.1
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.15
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+publish_visualization : True
+load_landmarks : True
+landmarks_pos_fixed : False
+search_for_new_landmarks : False
+first_robot_state_fixed : False
+last_robot_state_fixed : False
+all_robot_states_fixed : True
+amcl_localization : True
+calculate_covariance : False
+publish_tf_map_odom : False
+
+update_problem_rate: 0.1
+update_problem_distance: 0.2
+update_problem_angle: 0.1
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.15
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/adc_common/landmarks_loc/localization.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+publish_visualization : True
+load_landmarks : True
+landmarks_pos_fixed : True
+search_for_new_landmarks : False
+first_robot_state_fixed : False
+last_robot_state_fixed : False
+all_robot_states_fixed : False
+amcl_localization : False
+calculate_covariance : True
+publish_tf_map_odom : True
+
+update_problem_rate: 0.1
+update_problem_distance: 0.2
+update_problem_angle: 0.1
+problem_frame_window: 60
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.15
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/adc_common/landmarks_loc/slam_mapping.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+publish_visualization : True
+load_landmarks : False
+landmarks_pos_fixed : False
+search_for_new_landmarks : True
+first_robot_state_fixed : True
+last_robot_state_fixed : False
+all_robot_states_fixed : False
+amcl_localization : False
+calculate_covariance : False
+publish_tf_map_odom : True
+
+update_problem_rate: 0.1
+update_problem_distance: 0.2
+update_problem_angle: 0.1
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.15
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

launch/adc_global_localization.launch

@@ -16,6 +16,10 @@
   <arg name="map_path"                    default="$(find iri_adc_launch)/maps"/>
   <arg name="map_name"                    default="adc_map" />
   <arg name="amcl_config"                 default="$(find iri_adc_launch)/config/adc_rosnav/amcl_no_tf.yaml"/>
+  <arg name="landmarks_config_dir"        default="$(find iri_adc_launch)/config/adc_common/landmarks_loc"/>
+  <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"/>
 
   <!-- Ar track alvar (bringup??) -->
   <group ns="$(arg car_name)">
@@ -28,7 +32,7 @@
       <param name="max_new_marker_error"  type="double" value="0.08" />
       <param name="max_track_error"       type="double" value="0.2" />
       <param name="output_frame"          type="string" value="$(arg car_name)/front_camera_uvc_camera_optical" />
-      <remap from="/$(arg car_name)/camera_image"     to="/$(arg car_name)/sensors/basler_camera/image_raw" />
+      <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" />
     </node>
@@ -37,10 +41,29 @@
       <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_tag_localization_filter)/config/params.yaml"/>
+      <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"/>
     </include>
+    
+    <include file="$(find iri_image_inverter)/launch/node.launch">
+      <arg name="node_name"         value="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/image_raw"/>
+      <arg name="image_out_topic"   value="/$(arg car_name)/sensors/basler_camera/inverted_image_raw"/>
+    </include>
+
+    <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)"/>
+      <arg name="launch_prefix"              value="$(arg launch_prefix)"/>
+      <arg name="config_file"                value="$(arg landmarks_config_dir)/$(arg landmarks_config_file)"/>
+      <arg name="landmarks_map_file"         value="$(arg landmarks_data_dir)/$(arg landmarks_file)"/>
+      <arg name="estimated_pose_topic_name"  value="/$(arg car_name)/initialpose"/>
+      <arg name="features_topic_name"        value="/$(arg car_name)/iri_adc_tag_localization_filter/features"/>
+    </include>
+
   </group>
 
   <include file="$(find iri_rosnav)/launch/include/map_server.launch">

rviz/localization_global.rviz

@@ -615,7 +615,7 @@ Visualization Manager:
           Topic: /adc_car/global_odom_combined
           Unreliable: false
           Value: true
-      Enabled: false
+      Enabled: true
       Name: Odometry
     - Class: rviz/Group
       Displays:
@@ -758,7 +758,7 @@ Visualization Manager:
           Marker Topic: /adc_car/iri_adc_landmarks_slam_solver/landmarks
           Name: Landmarks
           Namespaces:
-            {}
+            landmarks: true
           Queue Size: 100
           Value: true
         - Class: rviz/MarkerArray
@@ -766,14 +766,15 @@ Visualization Manager:
           Marker Topic: /adc_car/iri_adc_landmarks_slam_solver/frame_data
           Name: Frames
           Namespaces:
-            {}
+            landmark_connection: true
+            odom_marker: true
           Queue Size: 100
           Value: true
         - Alpha: 1
           Axes Length: 1
           Axes Radius: 0.10000000149011612
           Class: rviz/PoseWithCovariance
-          Color: 255; 25; 0
+          Color: 92; 53; 102
           Covariance:
             Orientation:
               Alpha: 0.5