Commit 7b2c3e1b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the configuration of the landmark global localization.

Solved some bugs in the local and global localization EKF frameworks.
Updated some navigation parameters.
parent c74aa7c7
...@@ -5,7 +5,7 @@ speed_Kd: 0.00 ...@@ -5,7 +5,7 @@ speed_Kd: 0.00
speed_i_max: 20.0 speed_i_max: 20.0
speed_deadband: 0.1 speed_deadband: 0.1
watchdog_time: 0.5 watchdog_time: 0.5
axel_distance: 0.36 axel_distance: 0.3662
max_steer_angle: 0.5 max_steer_angle: 0.5
min_steer_angle: -0.5 min_steer_angle: -0.5
max_speed_control: 20.0 max_speed_control: 20.0
......
...@@ -5,33 +5,26 @@ two_d_mode: true ...@@ -5,33 +5,26 @@ two_d_mode: true
print_diagnostics: true print_diagnostics: true
debug: false debug: false
debug_out_file: /tmp/debug_ekf_localization.txt debug_out_file: /tmp/debug_ekf_localization.txt
use_control: true use_control: false
control_config: [true, true, false, false, false, true] control_config: [true, true, false, false, false, true]
publish_tf: false publish_tf: true
map_frame: map map_frame: map
odom_frame: adc_car/odom odom_frame: adc_car/odom
base_link_frame: adc_car/base_footprint base_link_frame: adc_car/base_footprint
world_frame: map world_frame: map
odom0: /adc_car/odom odom0: /adc_car/local_odom_combined
odom0_config: [false, false, false, odom0_config: [false, false, false,
false, false, false, false, false, false,
true, false, false, true, true, false,
false, false, true, false, false, true,
false, false, false] false, false, false]
# imu0: /adc_car/sensors/imu_data imu0_differential: false
# imu0_config: [false, false, false, imu0_relative: true
# false, false, false, imu0_remove_gravitational_acceleration: true
# false, false, false,
# false, false, true,
# false, false, false]
# imu0_differential: false
# imu0_relative: true
#imu0_remove_gravitational_acceleration: true
pose0: /adc_car/amcl_pose pose0: /adc_car/amcl_pose
pose0_config: [true, true, false, pose0_config: [true, true, false,
...@@ -45,12 +38,19 @@ pose0_relative: false ...@@ -45,12 +38,19 @@ pose0_relative: false
#pose0_rejection_threshold: 2 # Note the difference in parameter name #pose0_rejection_threshold: 2 # Note the difference in parameter name
#pose0_nodelay: false #pose0_nodelay: false
#odom1: /adc_car/visual_odometry pose1: /adc_car/iri_adc_landmarks_slam_solver/landmarks_localization_pose
#odom1_config: [false, false, false, pose1_config: [true, true, false,
#false, false, false, false, false, false,
#true, true, false, false, false, false,
#false, false, true, 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, #meaning_config: [x, y, z,
# roll, pitch, yaw, # roll, pitch, yaw,
......
...@@ -17,19 +17,30 @@ world_frame: adc_car/odom ...@@ -17,19 +17,30 @@ world_frame: adc_car/odom
odom0: /adc_car/odom odom0: /adc_car/odom
odom0_config: [false, false, false, odom0_config: [false, false, false,
false, false, false, false, false, false,
true, false, false, true, true, false,
false, false, true, false, false, true,
false, false, false] false, false, false]
imu0: /adc_car/sensors/imu_data #imu0: /adc_car/sensors/imu_data
#imu0_config: [false, false, false,
# false, false, true,
# false, false, false,
# false, false, false,
# false, false, false]
#imu0_differential: false
#imu0_relative: true
#imu0_remove_gravitational_acceleration: true
imu0: /adc_car/sensors/imu
imu0_config: [false, false, false, imu0_config: [false, false, false,
false, false, true,
false, false, false, false, false, false,
false, false, true, false, false, false,
false, false, false] true, true, true,
true, true, true]
imu0_differential: false imu0_differential: false
imu0_relative: true imu0_relative: false
imu0_remove_gravitational_acceleration: true imu0_remove_gravitational_acceleration: true
#meaning_config: [x, y, z, #meaning_config: [x, y, z,
......
rate: 10
roi_x_offset: 0
roi_y_offset: 320
roi_width: 1280
roi_height: 320
output_format: "mono8"
rate: 10
roi_x_offset: 0
roi_y_offset: 320
roi_width: 1280
roi_height: 256
output_format: "mono8"
...@@ -21,8 +21,8 @@ calculate_covariance: False ...@@ -21,8 +21,8 @@ calculate_covariance: False
publish_tf_map_odom: True publish_tf_map_odom: True
update_problem_rate: 0.001 update_problem_rate: 0.001
update_problem_distance: 0.2 update_problem_distance: 0.07
update_problem_angle: 0.1 update_problem_angle: 0.07
update_problem_features_detected: 3 update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2 wait_feature_detected_timeout: 0.2
problem_frame_window: -1 problem_frame_window: -1
......
...@@ -21,8 +21,8 @@ calculate_covariance: False ...@@ -21,8 +21,8 @@ calculate_covariance: False
publish_tf_map_odom: False publish_tf_map_odom: False
update_problem_rate: 0.001 update_problem_rate: 0.001
update_problem_distance: 0.2 update_problem_distance: 0.07
update_problem_angle: 0.1 update_problem_angle: 0.07
update_problem_features_detected: 3 update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2 wait_feature_detected_timeout: 0.2
problem_frame_window: -1 problem_frame_window: -1
......
...@@ -18,14 +18,14 @@ last_robot_state_estimated: False ...@@ -18,14 +18,14 @@ last_robot_state_estimated: False
all_robot_states_estimated: False all_robot_states_estimated: False
amcl_localization: False amcl_localization: False
calculate_covariance: True calculate_covariance: True
publish_tf_map_odom: True publish_tf_map_odom: False
update_problem_rate: 0.001 update_problem_rate: 0.001
update_problem_distance: 0.2 update_problem_distance: 0.07
update_problem_angle: 0.1 update_problem_angle: 0.07
update_problem_features_detected: 3 update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2 wait_feature_detected_timeout: 0.2
problem_frame_window: 60 problem_frame_window: 120
landmarks_candidates_filter_en: True landmarks_candidates_filter_en: True
landmark_mahalanobis_dist: 3.5 landmark_mahalanobis_dist: 3.5
......
...@@ -21,8 +21,8 @@ calculate_covariance: False ...@@ -21,8 +21,8 @@ calculate_covariance: False
publish_tf_map_odom: True publish_tf_map_odom: True
update_problem_rate: 0.001 update_problem_rate: 0.001
update_problem_distance: 0.2 update_problem_distance: 0.07
update_problem_angle: 0.1 update_problem_angle: 0.07
update_problem_features_detected: 3 update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2 wait_feature_detected_timeout: 0.2
problem_frame_window: -1 problem_frame_window: -1
......
...@@ -11,8 +11,8 @@ min_particles: 1000 ...@@ -11,8 +11,8 @@ min_particles: 1000
max_particles: 5000 max_particles: 5000
kld_err: 0.01 kld_err: 0.01
kld_z: 0.99 kld_z: 0.99
update_min_d: 0.1 update_min_d: 0.01
update_min_a: 0.1 update_min_a: 0.01
resample_interval: 2 resample_interval: 2
transform_tolerance: 0.1 transform_tolerance: 0.1
recovery_alpha_slow: 0.0 recovery_alpha_slow: 0.0
...@@ -38,9 +38,9 @@ laser_likelihood_max_dist: 2.0 ...@@ -38,9 +38,9 @@ laser_likelihood_max_dist: 2.0
laser_model_type: 'likelihood_field' laser_model_type: 'likelihood_field'
# odom parameters # odom parameters
odom_model_type: "diff" odom_model_type: "diff"
odom_alpha1: 0.2 odom_alpha1: 10.0
odom_alpha2: 0.2 odom_alpha2: 10.0
odom_alpha3: 0.02 odom_alpha3: 0.2
odom_alpha4: 0.2 odom_alpha4: 10.0
odom_alpha5: 0.2 odom_alpha5: 0.2
tf_broadcast: false tf_broadcast: false
...@@ -34,8 +34,8 @@ obstacle_layer: ...@@ -34,8 +34,8 @@ obstacle_layer:
min_obstacle_height: -0.2, min_obstacle_height: -0.2,
max_obstacle_height: 1.0, max_obstacle_height: 1.0,
expected_update_rate: 1, expected_update_rate: 1,
obstacle_range: 30.0, obstacle_range: 5.0,
raytrace_range: 40.0 raytrace_range: 10.0
} }
inflation_layer: inflation_layer:
......
base_local_planner: "AckermannPlannerROS" base_local_planner: "AckermannPlannerROS"
AckermannPlannerROS: AckermannPlannerROS:
max_sim_time: 3.0 max_sim_time: 1.75
min_sim_time: 1.0 min_sim_time: 1.75
sim_granularity: 0.2 sim_granularity: 0.05
angular_sim_granularity: 0.02 angular_sim_granularity: 0.01
path_distance_bias: 50.0 path_distance_bias: 50.0
goal_distance_bias: 10.0 goal_distance_bias: 10.0
occdist_scale: 0.05 occdist_scale: 0.05
# stop_time_buffer:
oscillation_reset_dist: 0.002 oscillation_reset_dist: 0.002
oscillation_reset_angle: 0.001 oscillation_reset_angle: 0.001
# forward_point_distance:
# scaling_speed:
# max_scaling_factor:
trans_vel_samples: 31 trans_vel_samples: 31
steer_angle_samples: 21 steer_angle_samples: 21
angular_vel_samples: 21
use_steer_angle_cmd: true
prune_plan: true prune_plan: true
xy_goal_tolerance: 0.1 xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.5 yaw_goal_tolerance: 0.2
trans_stopped_vel: 0.1 trans_stopped_vel: 0.1
rot_stopped_vel: 0.1 rot_stopped_vel: 0.1
restore_defaults: false restore_defaults: false
max_trans_vel: 1.0 max_trans_vel: 1.0
min_trans_vel: 0.11 min_trans_vel: 0.0
max_trans_acc: 0.2 max_trans_acc: 0.5
max_steer_angle: 0.5 max_steer_angle: 0.46
min_steer_angle: -0.5 min_steer_angle: -0.46
max_steer_vel: 0.3 max_steer_vel: 0.3
min_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 axis_distance: 0.36
wheel_distance: 0.265 wheel_distance: 0.265
wheel_radius: 0.05 wheel_radius: 0.05
use_trans_vel_deadzone: True
trans_vel_deadzone: 0.1
split_ignore_length: 0.1 split_ignore_length: 0.1
cmd_vel_avg: 1 cmd_vel_avg: 1
...@@ -49,5 +50,5 @@ AckermannPlannerROS: ...@@ -49,5 +50,5 @@ AckermannPlannerROS:
planner_patience: 5 planner_patience: 5
hdiff_scale: 0.5 hdiff_scale: 1.0
heading_points: 8 heading_points: 8
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<launch> <launch>
<arg name="car_name" default="adc_car"/> <arg name="car_name" default="adc_car"/>
<arg name="marker_size" default="5.0"/> <arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="marker_size" default="5.0"/>
<arg name="marker_margin" default="1.0"/>
<arg name="output" default="$(arg output)"/>
<arg name="launch_prefix" default="$(arg launch_prefix)"/>
<arg name="marker_size" default="5.0"/>
<arg name="marker_margin" default="1.0"/>
<arg name="basler_roi_config_file" default="$(find iri_adc_launch)/config/adc_common/basler_roi_config.yaml"/>
<include file="$(find iri_image_roi)/launch/node.launch">
<arg name="node_name" value="iri_image_roi_basler"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg basler_roi_config_file)"/>
<arg name="image_raw_in_topic" value="/$(arg car_name)/sensors/basler_camera/image_raw"/>
<arg name="camera_info_in_topic" value="/$(arg car_name)/sensors/basler_camera/camera_info"/>
<arg name="image_raw_out_topic" value="/$(arg car_name)/sensors/basler_camera_roi/image_raw"/>
<arg name="camera_info_out_topic" value="/$(arg car_name)/sensors/basler_camera_roi/camera_info"/>
</include>
<node name="ar_track_alvar" <node name="ar_track_alvar"
pkg="ar_track_alvar" pkg="ar_track_alvar"
type="individualMarkersNoKinect" type="individualMarkersNoKinect"
respawn="false" respawn="false"
output="screen"> output="$(arg output)">
<param name="marker_size" type="double" value="$(arg marker_size)" /> <param name="marker_size" type="double" value="$(arg marker_size)" />
<param name="marker_margin" type="double" value="$(arg marker_margin)" />
<param name="max_new_marker_error" type="double" value="0.08" /> <param name="max_new_marker_error" type="double" value="0.08" />
<param name="max_track_error" type="double" value="0.2" /> <param name="max_track_error" type="double" value="0.2" />
<param name="max_frequency" type="double" value="30.0" />
<param name="output_frame" type="string" value="$(arg car_name)/front_camera_uvc_camera_optical" /> <param name="output_frame" type="string" value="$(arg car_name)/front_camera_uvc_camera_optical" />
<remap from="/camera_image" to="$(arg car_name)/sensors/basler_camera/image_raw" /> <remap from="/camera_image" to="$(arg car_name)/sensors/basler_camera_roi/image_raw" />
<remap from="/camera_info" to="$(arg car_name)/sensors/basler_camera/camera_info" /> <remap from="/camera_info" to="$(arg car_name)/sensors/basler_camera_roi/camera_info" />
</node> </node>
</launch> </launch>
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<launch> <launch>
<arg name="car_name" default="adc_car"/> <arg name="car_name" default="adc_car"/>
<arg name="marker_size" default="5.0"/> <arg name="output" default="screen"/>
<arg name="marker_margin" default="1.0"/> <arg name="launch_prefix" default=""/>
<arg name="marker_size" default="5.0"/>
<arg name="marker_margin" default="1.0"/>
<arg name="basler_roi_config_file" default="$(find iri_adc_launch)/config/adc_common/basler_roi_config.yaml"/>
<include file="$(find iri_image_roi)/launch/node.launch">
<arg name="node_name" value="iri_image_roi_basler"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg basler_roi_config_file)"/>
<arg name="image_raw_in_topic" value="/$(arg car_name)/sensors/basler_camera/image_raw"/>
<arg name="camera_info_in_topic" value="/$(arg car_name)/sensors/basler_camera/camera_info"/>
<arg name="image_raw_out_topic" value="/$(arg car_name)/sensors/basler_camera_roi/image_raw"/>
<arg name="camera_info_out_topic" value="/$(arg car_name)/sensors/basler_camera_roi/camera_info"/>
</include>
<include file="$(find iri_image_inverter)/launch/node.launch"> <include file="$(find iri_image_inverter)/launch/node.launch">
<arg name="node_name" value="iri_image_inverter_basler"/> <arg name="node_name" value="iri_image_inverter_basler"/>
<arg name="image_in_topic" value="/$(arg car_name)/sensors/basler_camera/image_raw"/> <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_roi/image_raw"/>
<arg name="image_out_topic" value="/$(arg car_name)/sensors/basler_camera_inverse/image_raw"/> <arg name="image_out_topic" value="/$(arg car_name)/sensors/basler_camera_inverse/image_raw"/>
</include> </include>
...@@ -14,11 +30,12 @@ ...@@ -14,11 +30,12 @@
pkg="ar_track_alvar" pkg="ar_track_alvar"
type="individualMarkersNoKinect" type="individualMarkersNoKinect"
respawn="false" respawn="false"
output="screen"> output="$(arg output)">
<param name="marker_size" type="double" value="$(arg marker_size)" /> <param name="marker_size" type="double" value="$(arg marker_size)" />
<param name="marker_margin" type="double" value="$(arg marker_margin)" /> <param name="marker_margin" type="double" value="$(arg marker_margin)" />
<param name="max_new_marker_error" type="double" value="0.08" /> <param name="max_new_marker_error" type="double" value="0.08" />
<param name="max_track_error" type="double" value="0.2" /> <param name="max_track_error" type="double" value="0.2" />
<param name="max_frequency" type="double" value="30.0" />
<param name="output_frame" type="string" value="$(arg car_name)/front_camera_uvc_camera_optical" /> <param name="output_frame" type="string" value="$(arg car_name)/front_camera_uvc_camera_optical" />
<remap from="/camera_image" to="$(arg car_name)/sensors/basler_camera_inverse/image_raw" /> <remap from="/camera_image" to="$(arg car_name)/sensors/basler_camera_inverse/image_raw" />
<remap from="/camera_info" to="$(arg car_name)/sensors/basler_camera_inverse/camera_info" /> <remap from="/camera_info" to="$(arg car_name)/sensors/basler_camera_inverse/camera_info" />
......
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="car_name" default="adc_car"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="dr" default="false"/>
<!--Ar track alvar parameters-->
<arg name="marker_size" default="4.8"/>
<arg name="ar_max_freq" default="30.0"/>
<!-- Img ROI parameters -->
<arg name="front_roi_config_file" default="$(find iri_adc_launch)/config/adc_common/basler_roi_config.yaml"/>
<arg name="rear_roi_config_file" default="$(find iri_adc_launch)/config/adc_common/delock_roi_config.yaml"/>
<!-- Global localization parameters-->
<arg name="car_x" default="0.0"/>
<arg name="car_y" default="0.0"/>
<arg name="car_yaw" default="0.0"/>
<arg name="global_ekf_config_file" default="$(find iri_adc_launch)/config/adc_common/adc_global_ekf_odom_imu_amcl.yaml"/>
<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"/>
<arg name="use_rear_camera_loc" default="false"/>
<group ns="$(arg car_name)">
<!-- Front image ROI -->
<include file="$(find iri_image_roi)/launch/node.launch">
<arg name="node_name" value="front_iri_image_roi"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg front_roi_config_file)"/>
<arg name="image_raw_in_topic" value="/$(arg car_name)/sensors/basler_camera/image_raw"/>
<arg name="camera_info_in_topic" value="/$(arg car_name)/sensors/basler_camera/camera_info"/>
<arg name="image_raw_out_topic" value="/$(arg car_name)/sensors/basler_camera_roi/image_raw"/>
<arg name="camera_info_out_topic" value="/$(arg car_name)/sensors/basler_camera_roi/camera_info"/>
</include>
<!-- Front image inverter -->
<include file="$(find iri_image_inverter)/launch/node.launch">
<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_roi/image_raw"/>
<arg name="image_out_topic" value="/$(arg car_name)/sensors/basler_camera_inverse/image_raw"/>
</include>
<!-- Front ar track_alvar -->
<node name="front_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_frequency" type="double" value="$(arg ar_max_freq)" />
<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)/front_camera_uvc_camera_optical" />
<remap from="/$(arg car_name)/camera_image" to="/$(arg car_name)/sensors/basler_camera_inverse/image_raw" />
<remap from="/$(arg car_name)/camera_info" to="/$(arg car_name)/sensors/basler_camera_inverse/camera_info" />
<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="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 car_name)/front_features"/>
</include>
<group if="$(arg use_rear_camera_loc)">
<!-- Rear image ROI -->
<include file="$(find iri_image_roi)/launch/node.launch">
<arg name="node_name" value="rear_iri_image_roi"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg rear_roi_config_file)"/>
<arg name="image_raw_in_topic" value="/$(arg car_name)/sensors/delock_camera/image_raw"/>
<arg name="camera_info_in_topic" value="/$(arg car_name)/sensors/delock_camera/camera_info"/>
<arg name="image_raw_out_topic" value="/$(arg car_name)/sensors/delock_camera_roi/image_raw"/>
<arg name="camera_info_out_topic" value="/$(arg car_name)/sensors/delock_camera_roi/camera_info"/>
</include>