Merge branch 'sign_localization' into 'master'

Updated the configuration of the landmark global localization.

See merge request !3

config/adc/delock_camera_config.yaml

 width: 1280
 height: 960
 frame_id: 'adc_car/rear_camera_uvc_camera_optical'
-video_mode: 'mjpeg'
-frame_rate: 30.0
+video_mode: 'yuyv'
+frame_rate: 7.5

config/adc_common/adc_control_config.yaml

@@ -5,7 +5,7 @@ speed_Kd: 0.00
 speed_i_max: 20.0
 speed_deadband: 0.1
 watchdog_time: 0.5
-axel_distance: 0.36
+axel_distance: 0.3662
 max_steer_angle: 0.5
 min_steer_angle: -0.5
 max_speed_control: 20.0

config/adc_common/adc_global_ekf_odom_imu_amcl.yaml

@@ -5,33 +5,26 @@ two_d_mode: true
 print_diagnostics: true
 debug: false
 debug_out_file: /tmp/debug_ekf_localization.txt
-use_control: true
+use_control: false
 control_config: [true, true, false, false, false, true]
 
 
-publish_tf: true
+publish_tf: true 
 map_frame: map
 odom_frame: adc_car/odom
 base_link_frame: adc_car/base_footprint
 world_frame: map
 
-odom0: /adc_car/odom
+odom0: /adc_car/local_odom_combined
 odom0_config: [false, false, false,
                false, false, false,
-               true,  false,  false,
+               true,  true,  false,
                false, false, true,
                false, false, false]
 
-imu0: /adc_car/sensors/imu_data
-imu0_config: [false, false, false,
-              false, false, false,
-              false, false, false,
-              false, false, true,
-              false,  false, false]
-
 imu0_differential: false
 imu0_relative: true
-#imu0_remove_gravitational_acceleration: true
+imu0_remove_gravitational_acceleration: true
 
 pose0: /adc_car/amcl_pose
 pose0_config: [true,  true,  false,
@@ -45,12 +38,19 @@ pose0_relative: false
 #pose0_rejection_threshold: 2  # Note the difference in parameter name
 #pose0_nodelay: false
 
-#odom1: /adc_car/visual_odometry
-#odom1_config: [false, false, false,
-               #false, false, false,
-               #true,  true,  false,
-               #false, false, true,
-               #false, false, false]
+pose1: /adc_car/iri_adc_landmarks_slam_solver/landmarks_localization_pose
+pose1_config: [true,  true,  false,
+               false, false, false,
+               false, false, false,
+               false, false, false,
+               false, false, false]
+pose1_differential: true
+pose1_relative: false
+#pose1_queue_size: 5
+#pose1_rejection_threshold: 2  # Note the difference in parameter name
+#pose1_nodelay: false
+#
+
 
 #meaning_config: [x,     y,      z,
 #                 roll,  pitch,  yaw,

config/adc_common/adc_local_ekf_odom_imu.yaml

@@ -17,19 +17,19 @@ world_frame: adc_car/odom
 odom0: /adc_car/odom
 odom0_config: [false, false, false,
                false, false, false,
-               true,  false, false,
+               true,  true,  false,
                false, false, true,
                false, false, false]
 
-imu0: /adc_car/sensors/imu_data
+imu0: /adc_car/sensors/imu
 imu0_config: [false, false, false,
-              false, false, true,
               false, false, false,
-              false, false, true,
-              false,  false, false]
+              false, false, false,
+              true,  true,  true,
+              true,  true,  true]
 
 imu0_differential: false
-imu0_relative: true
+imu0_relative: false
 imu0_remove_gravitational_acceleration: true
 
 #meaning_config: [x,     y,      z,

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_filter_orientation_en: True
+time_persistance_alpha_window: 0.15
+time_persistance_range_window: 0.2
+time_persistance_orientation_th: 0.4
+time_persistance_min_detections: 4

config/adc_common/basler_roi_config.yaml

+rate: 10
+roi_x_offset: 0
+roi_y_offset: 320
+roi_width: 1280
+roi_height: 320
+output_format: "mono8"
+

config/adc_common/crop_front_image.yaml

+y_offset: 480
+height: 320

config/adc_common/delock_roi_config.yaml

+rate: 10
+roi_x_offset: 0
+roi_y_offset: 320
+roi_width: 1280
+roi_height: 256
+output_format: "mono8"

config/adc_common/landmarks_loc/amcl_mapping.yaml

+rate: 20.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+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
+publish_pose_angle: 0.1
+
+publish_visualization: True
+load_landmarks: False
+landmarks_pos_fixed: False
+search_for_new_landmarks: True
+first_robot_state_estimated: False
+last_robot_state_estimated: False
+all_robot_states_estimated: True
+amcl_localization: True
+calculate_covariance: False
+publish_tf_map_odom: False
+
+update_problem_rate: 0.001
+update_problem_distance: 0.2
+update_problem_angle: 0.1
+update_problem_features_detected: 3
+wait_feature_detected_timeout: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.3
+
+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: 20.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+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
+publish_pose_angle: 0.1
+
+publish_visualization: True
+load_landmarks: True
+landmarks_pos_fixed: False
+search_for_new_landmarks: False
+first_robot_state_estimated: True
+last_robot_state_estimated: False
+all_robot_states_estimated: False
+amcl_localization: False
+calculate_covariance: False
+publish_tf_map_odom: True
+
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
+update_problem_features_detected: 3
+wait_feature_detected_timeout: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.3
+
+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: 20.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+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
+publish_pose_angle: 0.1
+
+publish_visualization: True
+load_landmarks: True
+landmarks_pos_fixed: False
+search_for_new_landmarks: False
+first_robot_state_estimated: False
+last_robot_state_estimated: False
+all_robot_states_estimated: True
+amcl_localization: True
+calculate_covariance: False
+publish_tf_map_odom: False
+
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
+update_problem_features_detected: 3
+wait_feature_detected_timeout: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.3
+
+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: 20.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+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
+publish_pose_angle: 0.1
+
+publish_visualization: True
+load_landmarks: True
+landmarks_pos_fixed: True
+search_for_new_landmarks: False
+first_robot_state_estimated: False
+last_robot_state_estimated: False
+all_robot_states_estimated: False
+amcl_localization: False
+calculate_covariance: True
+publish_tf_map_odom: False
+
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
+update_problem_features_detected: 3
+wait_feature_detected_timeout: 0.2
+problem_frame_window: 120
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.3
+
+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: 20.0
+global_frame: "map"
+odom_frame: "adc_car/odom"
+base_link_frame: "adc_car/base_link"
+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
+publish_pose_angle: 0.1
+
+publish_visualization: True
+load_landmarks: False
+landmarks_pos_fixed: False
+search_for_new_landmarks: True
+first_robot_state_estimated: True
+last_robot_state_estimated: False
+all_robot_states_estimated: False
+amcl_localization: False
+calculate_covariance: False
+publish_tf_map_odom: True
+
+update_problem_rate: 0.001
+update_problem_distance: 0.07
+update_problem_angle: 0.07
+update_problem_features_detected: 3
+wait_feature_detected_timeout: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 3.5
+landmarks_min_detections: 4
+landmarks_filter_orientation_en: True
+landmarks_orientation_th: 0.4
+
+sensor_sigma_r: 0.2
+sensor_sigma_th: 0.3
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/adc_rosnav/amcl_no_tf.yaml

@@ -11,8 +11,8 @@ min_particles: 1000
 max_particles: 5000
 kld_err: 0.01
 kld_z: 0.99
-update_min_d: 0.1
-update_min_a: 0.1
+update_min_d: 0.01
+update_min_a: 0.01
 resample_interval: 2
 transform_tolerance: 0.1
 recovery_alpha_slow: 0.0
@@ -38,9 +38,9 @@ laser_likelihood_max_dist: 2.0
 laser_model_type: 'likelihood_field'
 # odom parameters
 odom_model_type: "diff"
-odom_alpha1: 0.2
-odom_alpha2: 0.2
-odom_alpha3: 0.02
-odom_alpha4: 0.2
+odom_alpha1: 10.0
+odom_alpha2: 10.0
+odom_alpha3: 0.2
+odom_alpha4: 10.0
 odom_alpha5: 0.2
 tf_broadcast: false

config/adc_rosnav/costmap/common_params.yaml

@@ -34,8 +34,8 @@ obstacle_layer:
     min_obstacle_height: -0.2,
     max_obstacle_height: 1.0,
     expected_update_rate: 1,
-    obstacle_range: 30.0,
-    raytrace_range: 40.0
+    obstacle_range: 5.0,
+    raytrace_range: 10.0
   }
 
 inflation_layer:

config/adc_rosnav/local_planner/ackermann_params.yaml

 base_local_planner: "AckermannPlannerROS"
 
 AckermannPlannerROS:
-  max_sim_time: 3.0
-  min_sim_time: 1.0
-  sim_granularity: 0.2
-  angular_sim_granularity: 0.02
+  max_sim_time: 1.75
+  min_sim_time: 1.75
+  sim_granularity: 0.05
+  angular_sim_granularity: 0.01
 
   path_distance_bias: 50.0
   goal_distance_bias: 10.0
   occdist_scale: 0.05
 
-#  stop_time_buffer: 
   oscillation_reset_dist: 0.002
   oscillation_reset_angle: 0.001
 
-#  forward_point_distance:
-
-#  scaling_speed:
-#  max_scaling_factor:
-
   trans_vel_samples: 31
   steer_angle_samples: 21
+  angular_vel_samples: 21
+  use_steer_angle_cmd: true
 
   prune_plan: true
   xy_goal_tolerance: 0.1
-  yaw_goal_tolerance: 0.5
+  yaw_goal_tolerance: 0.2
   trans_stopped_vel: 0.1
   rot_stopped_vel: 0.1
 
   restore_defaults: false
 
   max_trans_vel: 1.0
-  min_trans_vel: 0.11
-  max_trans_acc: 0.2
-  max_steer_angle: 0.5
-  min_steer_angle: -0.5
+  min_trans_vel: 0.0
+  max_trans_acc: 0.5
+  max_steer_angle: 0.46
+  min_steer_angle: -0.46
   max_steer_vel: 0.3
   min_steer_vel: -0.3
-  max_steer_acc: 1.0
+  max_steer_acc: 5.0
+  max_angular_vel: 0.5
+  min_angular_vel: -0.5
   axis_distance: 0.36
   wheel_distance: 0.265
   wheel_radius: 0.05
 
+  use_trans_vel_deadzone: True
+  trans_vel_deadzone: 0.1
+
   split_ignore_length: 0.1
 
   cmd_vel_avg: 1
@@ -49,5 +50,5 @@ AckermannPlannerROS:
 
   planner_patience: 5
 
-  hdiff_scale: 0.5
+  hdiff_scale: 1.0
   heading_points: 8

config/adc_sim/delock_camera_sim_config.yaml

 horizontal_fov: 1.4 
-width: 1024
-height: 768
+width: 1280
+height: 960
 format: 'R8G8B8'
 clip_near: 0.1
 clip_far: 100

data/lab_test/landmarks.txt

+101.0 1.374 1.0065 -0.7854 2
+102.0 1.426 1.0065 0.7854 2
+103.0 0.2015 1.484 -0.7854 2
+104.0 0.2015 1.536 -2.3562 2
+105.0 1.374 1.9735 -2.3562 2
+106.0 1.426 1.9735 2.3562 2
+107.0 2.3985 1.484 0.7854 2
+108.0 2.3985 1.536 2.3562 2
+109.0 1.374 2.9115 -0.7854 2
+110.0 1.426 2.9115 0.7854 2
+111.0 0.6715 4.429 -0.7854 2
+112.0 0.6715 4.481 -2.3562 2
+113.0 1.374 5.9985 -2.3562 2
+114.0 1.426 5.9985 2.3562 2
+115.0 1.5285 4.429 0.7854 2
+116.0 1.5285 4.481 2.3562 2
+117.0 1.374 -0.5215 -2.3562 2
+118.0 1.426 -0.5215 2.3562 2
+119.0 -1.2215 1.374 0.7854 2
+120.0 -1.2215 1.426 2.3562 2
+121.0 -1.2215 3.374 0.7854 2
+122.0 -1.2215 3.426 2.3562 2
+123.0 -1.2215 5.374 0.7854 2
+124.0 -1.2215 5.426 2.3562 2
+125.0 1.374 7.3215 -0.7854 2
+126.0 1.426 7.3215 0.7854 2
+127.0 3.8215 1.374 -0.7854 2
+128.0 3.8215 1.426 -2.3562 2
+129.0 3.8215 3.374 -0.7854 2
+130.0 3.8215 3.426 -2.3562 2
+131.0 3.8215 5.374 -0.7854 2
+132.0 3.8215 5.426 -2.3562 2