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
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
......
......@@ -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: false
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_differential: false
imu0_relative: 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,
......
......@@ -17,19 +17,30 @@ 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_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,
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,
......
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
publish_tf_map_odom: True
update_problem_rate: 0.001
update_problem_distance: 0.2
update_problem_angle: 0.1
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
......
......@@ -21,8 +21,8 @@ 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_distance: 0.07
update_problem_angle: 0.07
update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2
problem_frame_window: -1
......
......@@ -18,14 +18,14 @@ last_robot_state_estimated: False
all_robot_states_estimated: False
amcl_localization: False
calculate_covariance: True
publish_tf_map_odom: True
publish_tf_map_odom: False
update_problem_rate: 0.001
update_problem_distance: 0.2
update_problem_angle: 0.1
update_problem_distance: 0.07
update_problem_angle: 0.07
update_problem_features_detected: 3
wait_feature_detected_timeout: 0.2
problem_frame_window: 60
problem_frame_window: 120
landmarks_candidates_filter_en: True
landmark_mahalanobis_dist: 3.5
......
......@@ -21,8 +21,8 @@ calculate_covariance: False
publish_tf_map_odom: True
update_problem_rate: 0.001
update_problem_distance: 0.2
update_problem_angle: 0.1
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
......
......@@ -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
......@@ -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:
......
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
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="car_name" default="adc_car"/>
<arg name="marker_size" default="5.0"/>
<arg name="car_name" default="adc_car"/>
<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"
pkg="ar_track_alvar"
type="individualMarkersNoKinect"
respawn="false"
output="screen">
output="$(arg output)">
<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_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" />
<remap from="/camera_image" to="$(arg car_name)/sensors/basler_camera/image_raw" />
<remap from="/camera_info" to="$(arg car_name)/sensors/basler_camera/camera_info" />
<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_roi/camera_info" />
</node>
</launch>
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="car_name" default="adc_car"/>
<arg name="marker_size" default="5.0"/>
<arg name="marker_margin" default="1.0"/>
<arg name="car_name" default="adc_car"/>
<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="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">
<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"/>
</include>
......@@ -14,11 +30,12 @@
pkg="ar_track_alvar"
type="individualMarkersNoKinect"
respawn="false"
output="screen">
output="$(arg output)">
<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_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" />
<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" />
......
<?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>
<!-- Rear 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_roi/image_raw"/>
<arg name="image_out_topic" value="/$(arg car_name)/sensors/delock_camera_inverse/image_raw"/>
</include>
<!--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_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)/rear_camera_uvc_camera_optical" />
<remap from="/$(arg car_name)/camera_image" to="/$(arg car_name)/sensors/delock_camera_inverse/image_raw" />
<remap from="/$(arg car_name)/camera_info" to="/$(arg car_name)/sensors/delock_camera_inverse/camera_info" />
<remap from="/$(arg car_name)/ar_pose_marker" to="/$(arg car_name)/rear_ar_pose_marker" />
</node>
<!-- Rear 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 car_name)/rear_features"/>
</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)"/>
<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)/estimated_pose"/>
<arg name="initialpose_topic_name" value="/$(arg car_name)/initialpose"/>
<arg name="front_features_topic_name" value="/$(arg car_name)/front_features"/>
<arg name="rear_features_topic_name" value="/$(arg car_name)/rear_features"/>
<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)"/>
</include>
</group>
<node name="rqt_reconfigure_iri_adc_tag_localization_filter"
pkg ="rqt_reconfigure"
type="rqt_reconfigure"
if ="$(arg dr)"
args="iri_adc_tag_localization_filter">
</node>
</launch>
......@@ -879,4 +879,4 @@ Window Geometry:
collapsed: false
Width: 1853
X: 67
Y: 27
Y: 27
\ No newline at end of file
......@@ -6,13 +6,15 @@ Panels:
Expanded:
- /Global Options1
- /Models1
- /Sensors1
- /Sensors1/Sonar1
- /Sensors1/outliers_scan1
- /Odometry1/(red)OdomEncoders(1)1/Shape1
- /Odometry1/(green)OdomCombinedLocal(1)1/Shape1
- /Odometry1/(green)OdomCombinedLocal1/Shape1
- /Nav1
Splitter Ratio: 0.637188196182251
Tree Height: 641
Tree Height: 794
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
......@@ -306,6 +308,66 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 117; 80; 123
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
Min Color: 0; 0; 0
Min Intensity: 0
Name: inliers_scan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /adc_car/sensors/inliers_scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true