From 7b2c3e1baeaa9f4f1965fdc62f37f2081bb57d57 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 14 Sep 2021 10:10:46 +0200
Subject: [PATCH] Updated the configuration of the landmark global
localization. Solved some bugs in the local and global localization EKF
frameworks. Updated some navigation parameters.
---
config/adc_common/adc_control_config.yaml | 2 +-
.../adc_global_ekf_odom_imu_amcl.yaml | 40 ++---
config/adc_common/adc_local_ekf_odom_imu.yaml | 23 ++-
config/adc_common/basler_roi_config.yaml | 7 +
config/adc_common/delock_roi_config.yaml | 6 +
.../landmarks_loc/landmarks_calibration.yaml | 4 +-
.../landmarks_calibration_amcl.yaml | 4 +-
.../landmarks_loc/localization.yaml | 8 +-
.../landmarks_loc/slam_mapping.yaml | 4 +-
config/adc_rosnav/amcl_no_tf.yaml | 12 +-
config/adc_rosnav/costmap/common_params.yaml | 4 +-
.../local_planner/ackermann_params.yaml | 35 ++--
launch/adc_ar_detector.launch | 31 +++-
launch/adc_ar_detector_inverse.launch | 27 ++-
launch/adc_landmarks_localization.launch | 154 ++++++++++++++++++
rviz/localization_global.rviz | 2 +-
rviz/nav.rviz | 78 ++++++++-
17 files changed, 360 insertions(+), 81 deletions(-)
create mode 100644 config/adc_common/basler_roi_config.yaml
create mode 100644 config/adc_common/delock_roi_config.yaml
create mode 100644 launch/adc_landmarks_localization.launch
diff --git a/config/adc_common/adc_control_config.yaml b/config/adc_common/adc_control_config.yaml
index 2538247..cc0a54c 100644
--- a/config/adc_common/adc_control_config.yaml
+++ b/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
diff --git a/config/adc_common/adc_global_ekf_odom_imu_amcl.yaml b/config/adc_common/adc_global_ekf_odom_imu_amcl.yaml
index 1a9f450..35232f0 100644
--- a/config/adc_common/adc_global_ekf_odom_imu_amcl.yaml
+++ b/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: 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,
diff --git a/config/adc_common/adc_local_ekf_odom_imu.yaml b/config/adc_common/adc_local_ekf_odom_imu.yaml
index de8288d..2f95611 100644
--- a/config/adc_common/adc_local_ekf_odom_imu.yaml
+++ b/config/adc_common/adc_local_ekf_odom_imu.yaml
@@ -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,
diff --git a/config/adc_common/basler_roi_config.yaml b/config/adc_common/basler_roi_config.yaml
new file mode 100644
index 0000000..72174e1
--- /dev/null
+++ b/config/adc_common/basler_roi_config.yaml
@@ -0,0 +1,7 @@
+rate: 10
+roi_x_offset: 0
+roi_y_offset: 320
+roi_width: 1280
+roi_height: 320
+output_format: "mono8"
+
diff --git a/config/adc_common/delock_roi_config.yaml b/config/adc_common/delock_roi_config.yaml
new file mode 100644
index 0000000..43084de
--- /dev/null
+++ b/config/adc_common/delock_roi_config.yaml
@@ -0,0 +1,6 @@
+rate: 10
+roi_x_offset: 0
+roi_y_offset: 320
+roi_width: 1280
+roi_height: 256
+output_format: "mono8"
diff --git a/config/adc_common/landmarks_loc/landmarks_calibration.yaml b/config/adc_common/landmarks_loc/landmarks_calibration.yaml
index f726fed..869beea 100644
--- a/config/adc_common/landmarks_loc/landmarks_calibration.yaml
+++ b/config/adc_common/landmarks_loc/landmarks_calibration.yaml
@@ -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
diff --git a/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml b/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml
index 02cc8d1..a49cdf3 100644
--- a/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml
+++ b/config/adc_common/landmarks_loc/landmarks_calibration_amcl.yaml
@@ -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
diff --git a/config/adc_common/landmarks_loc/localization.yaml b/config/adc_common/landmarks_loc/localization.yaml
index 577d40a..b1c1039 100644
--- a/config/adc_common/landmarks_loc/localization.yaml
+++ b/config/adc_common/landmarks_loc/localization.yaml
@@ -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
diff --git a/config/adc_common/landmarks_loc/slam_mapping.yaml b/config/adc_common/landmarks_loc/slam_mapping.yaml
index 60b8e36..1442dc3 100644
--- a/config/adc_common/landmarks_loc/slam_mapping.yaml
+++ b/config/adc_common/landmarks_loc/slam_mapping.yaml
@@ -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
diff --git a/config/adc_rosnav/amcl_no_tf.yaml b/config/adc_rosnav/amcl_no_tf.yaml
index af186af..02cb42e 100644
--- a/config/adc_rosnav/amcl_no_tf.yaml
+++ b/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
diff --git a/config/adc_rosnav/costmap/common_params.yaml b/config/adc_rosnav/costmap/common_params.yaml
index 9d3714b..338f281 100644
--- a/config/adc_rosnav/costmap/common_params.yaml
+++ b/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:
diff --git a/config/adc_rosnav/local_planner/ackermann_params.yaml b/config/adc_rosnav/local_planner/ackermann_params.yaml
index 0286e32..847ec77 100644
--- a/config/adc_rosnav/local_planner/ackermann_params.yaml
+++ b/config/adc_rosnav/local_planner/ackermann_params.yaml
@@ -1,47 +1,48 @@
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
diff --git a/launch/adc_ar_detector.launch b/launch/adc_ar_detector.launch
index 4c7e016..b7c0f37 100644
--- a/launch/adc_ar_detector.launch
+++ b/launch/adc_ar_detector.launch
@@ -1,18 +1,39 @@
<?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>
diff --git a/launch/adc_ar_detector_inverse.launch b/launch/adc_ar_detector_inverse.launch
index 90d0c55..62a2761 100644
--- a/launch/adc_ar_detector_inverse.launch
+++ b/launch/adc_ar_detector_inverse.launch
@@ -1,12 +1,28 @@
<?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" />
diff --git a/launch/adc_landmarks_localization.launch b/launch/adc_landmarks_localization.launch
new file mode 100644
index 0000000..45dedff
--- /dev/null
+++ b/launch/adc_landmarks_localization.launch
@@ -0,0 +1,154 @@
+<?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>
diff --git a/rviz/localization_global.rviz b/rviz/localization_global.rviz
index 13b8fd3..f43a004 100644
--- a/rviz/localization_global.rviz
+++ b/rviz/localization_global.rviz
@@ -879,4 +879,4 @@ Window Geometry:
collapsed: false
Width: 1853
X: 67
- Y: 27
+ Y: 27
\ No newline at end of file
diff --git a/rviz/nav.rviz b/rviz/nav.rviz
index 4230645..81ad3b7 100644
--- a/rviz/nav.rviz
+++ b/rviz/nav.rviz
@@ -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
+ 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: 193; 125; 17
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 193; 125; 17
+ Max Intensity: 0
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: outliers_scan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /adc_car/sensors/outliers_scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
- Alpha: 0.25
Class: rviz_plugin_tutorials/Imu
Color: 204; 51; 204
@@ -796,16 +858,16 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 27.908039093017578
+ Distance: 20.100095748901367
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 1.1276541948318481
- Y: 2.8684005737304688
- Z: -1.8374912738800049
+ X: 3.2848408222198486
+ Y: 2.6993188858032227
+ Z: -1.6742990016937256
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
@@ -821,10 +883,10 @@ Window Geometry:
collapsed: false
FrontImage:
collapsed: false
- Height: 872
+ Height: 1025
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 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
+ QMainWindow State: 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
RearImage:
collapsed: false
Selection:
@@ -835,6 +897,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 1373
+ Width: 1853
X: 67
Y: 27
--
GitLab