Description

This node provides a global localization based on landmarks. On the ADC competition, these landmarks are ar_track_alvar tags from the traffic signs and from additional localization signs. This node takes the filtered ar tag detections (features) of the rear and front cameras and optimizes a landmark based slam problem to get the robot position. A landmarks based slam problem is the problem of optimize a number of robot poses with a map of landmarks. It uses Ceres Solver to make the optimization.

The following image is a skecth of the input and output of the node.

The following image is a basic representation of the slam problem. Is a sequence of robot poses with their landmarks detections connected by the odometry.

When searching for new landmarks, there is a time persistence filter that checks that a feature is seen enough times on the same location before adding it as a mapped landmarks. An orientation filter can be enabled to check that it has a similar orientation on each detection.

To match a detection with a landmark the Mahalanobis distance is used. Basically, a match between a detection and a landmark is done when the detection is inside an ellipse centered on the landmark. The ellipse is defined by the sensor noise and the mahalanobis distance parameter.

On parameters_adjust.md there is an small explanation on how to adjust the node parameters.

The landmarks information can be loaded in two ways:

1. From the xodr file: This is the opendrive file that describres the cirucit, including the localization signs. Landmarks on this file are on opendrive frame reference system.
2. From a txt files: This is a txt file with the landmarks information. The information on this file is on global frame reference system. Each line defines a landmark with the following fields space separated:

• id (double): Unique identifier of each landmark.
• x (double): Landmark X coordenate.
• y (double): Landmark Y coordenate.
• yaw (double): Landmark yaw.
• marker_id (integer): The ar_track_alvar marker id.

The landmarks txt file can be generated loading the landmarks from the xodr file and setting the parameter write_output_files on a Dynamic Reconfigure window.

If the landmarks are loaded from the xodr file, load_landmarks_from_xodr parameter must be set to true.

ROS Interface

Topic publishers

• ~frame_data (visualization_msgs/MarkerArray.msg): Visualization of the current slam problem. Red arrows represent the robot pose. Blue lines represent the landmarks seen from each robot pose.
• ~landmarks (visualization_msgs/MarkerArray.msg): Visualization of the landmarks map for debugging purposes. The blue ones are detected on the current time.
• ~landmarks_localization_pose (geometry_msgs/PoseWithCovarianceStamped.msg): Robot global estimated pose with covariance.
• /tf (tf/tfMessage): To publish the transformation between map and odom if enabled.

Topic subscribers

• ~initialpose (geometry_msgs/PoseWithCovarianceStamped.msg): Topic to initialize the slam problem with a robot initial pose.
• ~estimated_pose (geometry_msgs/PoseWithCovarianceStamped.msg): Topic to accept estimated poses from other localizations.
• /tf (tf/tfMessage): Tf to listen the transforms between odom an robot's base link and between the robot's base link and the sensors.
• ~front_features (iri_adc_msgs/feature_array.msg): Incoming detections from the front camera.
• ~rear_features (iri_adc_msgs/feature_array.msg): Incoming detections from the rear camera.

Parameters

General

• ~rate (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz.
• ~global_frame (String; default: map) Global frame id.
• ~odom_frame (String; default: odom) Odometry frame id.
• ~base_link_frame (String; default: base_link) Robot's base link frame id.
• ~opendrive_frame (String; default: opendrive) Opendrive frame id.
• ~tf_timeout (Double; default: 0.2; min: 0.1; max: 2.0) Timeout to find a transform.
• ~old_feature_timeout (Double; default: 0.5; min: 0.1; max: 2.0) Timeout to set a features source as old.
• ~amcl_pose_estimated_sigma (Double; default: 1.0; min: 0.1; max: 10.0) AMCL pose sigma when using AMCL localization.
• ~publish_pose_rate (Double; default:1.0; min: 0.00001; max: 10) Rate to publish the robot pose.
• ~publish_pose_distance (Double; default: 1.0; min: 0.1; max: 100000) Distance from last robot state to publish the robot pose.
• ~publish_pose_angle (Double; default: 0.2; min: 0.05; max: 3.14) Angle inc from last robot state to publish the robot pose.
• ~write_output_files (Boolean; default: False) Boolean to print landmarks info to a txt file.
• ~output_files_folder (String; default: ./) Output txt file path.
• ~err_msg_rate (Double; default: 0.5; min: 0.1; max: 1.0) Rate to publish error messages.
• ~add_frame_and_update (Boolean; default: False) Boolean to add current frame and optimize the solution.
• ~init_pose_covariance (Double; default: 0.01; min: 0.00001; max: 0.1) Initial pose covariance.

Landmark candidates filter

• ~landmarks_candidates_filter_en (Boolean; default: False) Boolean to filter landmarks checking if it's a feature on the following scans before adding it as landmark.
• ~landmarks_filter_orientation_en (Boolean; default: False) Boolean to add orientation filter to landmarks time persistence filter.
• ~landmark_mahalanobis_dist (Double; default: 2.0; min: 0.01; max: 20.0) Mahalonibis distance parameter.
• ~landmarks_min_detections (Integer; default: 3; min: 1; max: 30) The number of detections on a row to trust it as a new landmark.
• ~landmarks_orientation_th (Double; default: 0.4; min: 0.01; max: 0.8) Maximum orientation diff from the last detection on following inputs.

Ceres problem

• ~update_problem_rate (Double; default: 1.0; min: 0.00001; max: 10.0) Rate to update the ceres problem.
• ~update_problem_distance (Double; default: 1.0; min: 0.1; max: 1000000.0) Distance from last robot state to update the ceres problem.
• ~update_problem_angle (Double; default: 0.2; min: 0.05; max: 3.14) Angle inc from last robot state to update the ceres problem.
• ~update_problem_features_detected (Integer; default: 3; min: 1; max: 10) Update problem when a lot of features are detected.
• ~wait_feature_detected_timeout (Double; default: 0.2; min: 0.01; max: 0.5) Timeout on a update problem event to wait for a feature detection.
• ~landmarks_map_file (String; default: landmarks.txt) Landmarks map txt file path.
• ~load_landmarks_from_xodr (Boolean; default: False) Boolean to load landmarks from a xodr file.
• ~landmarks_xodr_scale (Double; default: 1.0; min: 0.0) Landmarks xodr scale factor.
• ~problem_frame_window (Integer; default: 120; min: 50; max: 1000) Max number of frames to add.

Flags

• ~publish_visualization (Boolean; default: False) Boolean to publish visualization markers.
• ~load_landmarks (Boolean; default: False) Boolean to load_landmarks from txt file.
• ~landmarks_pos_fixed (Boolean; default: False) Boolean to fix landmarks positions.
• ~search_for_new_landmarks (Boolean; default: False) Boolean to search for new landmarks.
• ~first_robot_state_estimated (Boolean; default: False) Boolean to add estimated residuals at the first robot position.
• ~last_robot_state_estimated (Boolean; default: False) Boolean to add estimated residuals at the last robot position.
• ~all_robot_states_estimated (Boolean; default: False) Boolean to add estimated residuals at all robot positions.
• ~amcl_localization (Boolean; default: False) Boolean to use amcl localization.
• ~calculate_covariance (Boolean; default: False) Boolean to calculate robot pose covariance.
• ~publish_tf_map_odom (Boolean; default: False) Boolean to publish tf from map to odom.

Sensor noise

• ~sensor_sigma_th (Double; default: 0.035; min: 0.001; max: 1.0) Sensor angular sigma.
• ~sensor_sigma_r (Double; default: 0.05; min: 0.01; max:1.0) Sensor radial sigma.

Odometry noise

• ~odom_fxy (Double; default: 0.05; min: 0.01; max: 1.0) Odom linear sigma factor.
• ~odom_fth (Double; default: 0.05; min: 0.01; max: 1.0) Odom angular sigma factor.
• ~odom_fxyth (Double; default: 0.05; min: 0.01; max: 1.0) Odom xyth sigma factor.
• ~odom_sigma_min (Double; default: 0.000001; min: 0.0000001; max: 1.0) Odom sigma min value.

Rviz visualization markers

This node publishes two different visualization markers:

• Landmarks: Rectangular parallelepiped that represents each landmark position and orientation. The cyan ones are detected at current time.
• Frame_data: This is a visualization of the SLAM problem. Red arrows are the poses of the robot at each frame. Cyan lines are the connection between the robot pose and the landmarks detected at the same frame.

Installation

Download the latest stable Ceres release and install it following the linux installation tutorial.

Move to the active workspace:

roscd && cd ../src

Clone the repository:

git clone https://gitlab.iri.upc.edu/mobile_robotics/adc/adc_2021/iri_adc_landmarks_slam_solver.git

Install ROS dependencies:

roscd
cd ..
rosdep install -i -r --from-paths src

Compile the workspace:

catkin_make

How to use it

This node provides a basic launch file named node.launch intended to be included on a general launch file with the following arguments:

• node_name: Node's name.
• output: ROS output parameter.
• launch_prefix: Node's launch prefix.
• config_file: Path to the configuration file.
• landmarks_map_file: Landmarks txt map file.
• load_landmarks_from_xodr: Flag to know if landmars are loaded from a xodr file.
• initial_pose_x: Initial robot x position.
• initial_pose_y: Initial robot y position.
• initial_pose_yaw: Initial robot orientation.
• estimated_pose_topic_name: Estimated pose topic name.
• initialpose_topic_name: Initial pose topic name.
• front_features_topic_name: Incoming front features topic name.
• rear_features_topic_name: Incoming rear features topic name.

It provides the following default configuration files on config directory:

• amcl_mapping.yaml: To map landmarks using AMCL localization.
• slam_mapping.yaml: To map landmarks without AMCL localization.
• localization.yaml: To localizate the robot.
• landmarks_calibration.yaml: To calibrate landmarks positions without AMCL localization.
• landmarks_calibration_amcl.yaml: To calibrate landmarks positionsn using AMCL localization.

It also provides an example of launch file named test.launch. It can be launch with the following command:

roslaunch iri_adc_landmarks_slam_solver test.launch

Disclaimer

Copyright (C) Institut de Robòtica i Informàtica Industrial, CSIC-UPC. Mantainer IRI labrobotics (labrobotica@iri.upc.edu)

This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.

In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.

You should have received a copy of the GNU Lesser General Public License along with this program. If not, see http://www.gnu.org/licenses/