以机器人为中心的高程映射,用于崎岖地形导航
[!NOTE] Elevation Mapping is no longer actively maintained.
This is a [ROS] package developed for elevation mapping with a mobile robot. The software is designed for (local) navigation tasks with robots which are equipped with a pose estimation (e.g. IMU & odometry) and a distance sensor (e.g. structured light (Kinect, RealSense), laser range sensor, stereo camera). The provided elevation map is limited around the robot and reflects the pose uncertainty that is aggregated through the motion of the robot (robot-centric mapping). This method is developed to explicitly handle drift of the robot pose estimation.
This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
The source code is released under a BSD 3-Clause license.
Author: Péter Fankhauser
Co-Author: Maximilian Wulf
Affiliation: ANYbotics
Maintainer: Maximilian Wulf, [email protected], Magnus Gärtner, [email protected]
This projected was initially developed at ETH Zurich (Autonomous Systems Lab & Robotic Systems Lab).
This work is conducted as part of ANYmal Research, a community to advance legged robotics.
Videos of the elevation mapping software in use:
The robot-centric elevation mapping methods used in this software are described in the following paper (available here). If you use this work in an academic context, please cite the following publication(s):
P. Fankhauser, M. Bloesch, and M. Hutter, "Probabilistic Terrain Mapping for Mobile Robots with Uncertain Localization", in IEEE Robotics and Automation Letters (RA-L), vol. 3, no. 4, pp. 3019–3026, 2018. (PDF)
@article{Fankhauser2018ProbabilisticTerrainMapping,
author = {Fankhauser, P{\'{e}}ter and Bloesch, Michael and Hutter, Marco},
doi = {10.1109/LRA.2018.2849506},
title = {Probabilistic Terrain Mapping for Mobile Robots with Uncertain Localization},
journal = {IEEE Robotics and Automation Letters (RA-L)},
volume = {3},
number = {4},
pages = {3019--3026},
year = {2018}
}
P. Fankhauser, M. Bloesch, C. Gehring, M. Hutter, and R. Siegwart, "Robot-Centric Elevation Mapping with Uncertainty Estimates", in International Conference on Climbing and Walking Robots (CLAWAR), 2014. (PDF)
@inproceedings{Fankhauser2014RobotCentricElevationMapping,
author = {Fankhauser, P\'{e}ter and Bloesch, Michael and Gehring, Christian and Hutter, Marco and Siegwart, Roland},
title = {Robot-Centric Elevation Mapping with Uncertainty Estimates},
booktitle = {International Conference on Climbing and Walking Robots (CLAWAR)},
year = {2014}
}
This software is built on the Robotic Operating System ([ROS]), which needs to be installed first. Additionally, the Robot-Centric Elevation Mapping depends on following software:
In order to install the Robot-Centric Elevation Mapping, clone the latest version from this repository into your catkin workspace and compile the package using ROS.
cd catkin_workspace/src
git clone https://github.com/anybotics/elevation_mapping.git
cd ../
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build
Build tests with
roscd elevation_mapping
catkin build --catkin-make-args run_tests -- --this
Run the tests with
rostest elevation_mapping elevation_mapping.test -t
In order to get the Robot-Centric Elevation Mapping to run with your robot, you will need to adapt a few parameters. It is the easiest if duplicate and adapt all the parameter files that you need to change from the elevation_mapping_demos package (e.g. the simple_demo example). These are specifically the parameter files in config and the launch file from the launch folder.
A running example is provided, making use of the Turtlebot3 simulation environment. This example can be used to test elevation mapping, as a starting point for further integration.
To start with, the Turtlebot3 simulation dependencies need to be installed:
sudo apt install ros-melodic-turtlebot3*
The elevation mapping demo together with the turtlebot3 simulation can be started with
roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch
To control the robot with a keyboard, a new terminal window needs to be opened (remember to source your ROS environment). Then run
export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
Velocity inputs can be sent to the robot by pressing the keys a, w,d, x. To stop the robot completely, press s.
A .ply is published as static pointcloud, elevation_mapping subscribes to it and publishes the elevation map. You can visualize it through rviz.
For visualization, select /elevation_mapping/elevation_map_raw.
Note. You might need to toggle the visibility of the grid_map_plugin to visualize it.
roslaunch elevation_mapping_demos ground_truth_demo.launch
While ground truth demo estimates the height in map frame, simple demo sets up a more realistic deployment scenario. Here, the elevation_map is configured to track a base frame.
To get started, we suggest to play around and also visualize other published topics, such as /elevation_mapping/elevation_map_raw and change the height layer to another layer, e.g elevation_inpainted.
This is the main Robot-Centric Elevation Mapping node. It uses the distance sensor measurements and the pose and covariance of the robot to generate an elevation map with variance estimates.
/points ([sensor_msgs/PointCloud2])
The distance measurements.
/pose ([geometry_msgs/PoseWithCovarianceStamped])
The robot pose and covariance.
/tf ([tf/tfMessage])
The transformation tree.
elevation_map ([grid_map_msgs/GridMap])
The entire (fused) elevation map. It is published periodically (see fused_map_publishing_rate parameter) or after the trigger_fusion service is called.
elevation_map_raw ([grid_map_msgs/GridMap])
The entire (raw) elevation map before the fusion step.
trigger_fusion ([std_srvs/Empty])
Trigger the fusing process for the entire elevation map and publish it. For example, you can trigger the map fusion step from the console with
rosservice call /elevation_mapping/trigger_fusion
get_submap ([grid_map_msgs/GetGridMap])
Get a fused elevation submap for a requested position and size. For example, you can get the fused elevation submap at position (-0.5, 0.0) and size (0.5, 1.2) described in the odom frame and save it to a text file form the console with
rosservice call -- /elevation_mapping/get_submap odom -0.5 0.0 0.5 1.2 []
get_raw_submap ([grid_map_msgs/GetGridMap])
Get a raw elevation submap for a requested position and size. For example, you can get the raw elevation submap at position (-0.5, 0.0) and size (0.5, 1.2) described in the odom frame and save it to a text file form the console with
rosservice call -- /elevation_mapping/get_raw_submap odom -0.5 0.0 0.5 1.2 []
clear_map ([std_srvs/Empty])
Initiates clearing of the entire map for resetting purposes. Trigger the map clearing with
rosservice call /elevation_mapping/clear_map
masked_replace ([grid_map_msgs/SetGridMap])
Allows for setting the individual layers of the elevation map through a service call. The layer mask can be used to only set certain cells and not the entire map. Cells containing NAN in the mask are not set, all the others are set. If the layer mask is not supplied, the entire map will be set in the intersection of both maps. The provided map can be of different size and position than the map that will be altered. An example service call to set some cells marked with a mask in the elevation layer to 0.5 is
rosservice call /elevation_mapping/masked_replace "map:
info:
header:
seq: 3
stamp: {secs: 3, nsecs: 80000000}
frame_id: 'odom'
resolution: 0.1
length_x: 0.3
length_y: 0.3
pose:
position: {x: 5.0, y: 0.0, z: 0.0}
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
layers: [elevation,mask]
basic_layers: [elevation]
data:
- layout:
dim:
- {label: 'column_index', size: 3, stride: 9}
- {label: 'row_index', size: 3, stride: 3}
data_offset: 0
data: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
- layout:
dim:
- {label: 'column_index', size: 3, stride: 9}
- {label: 'row_index', size: 3, stride: 3}
data_offset: 0
data: [0, 0, 0, .NAN, .NAN, .NAN, 0, 0, 0]
outer_start_index: 0
inner_start_index: 0"
save_map ([grid_map_msgs/ProcessFile])
Saves the current fused grid map and raw grid map to rosbag files. Field topic_name must be a base name, i.e. no leading slash character (/). If field topic_name is empty, then elevation_map is used per default. Example with default topic name
rosservice call /elevation_mapping/save_map "file_path: '/home/integration/elevation_map.bag' topic_name: ''"
load_map ([grid_map_msgs/ProcessFile])
Loads the fused grid map and raw grid map from rosbag files. Field topic_name must be a base name, i.e. no leading slash character (/). If field topic_name is empty, then elevation_map is used per default. Example with default topic name
rosservice call /elevation_mapping/load_map "file_path: '/home/integration/elevation_map.bag' topic_name: ''"
reload_parameters ([std_srvs/Trigger])
Triggers a re-load of all elevation mapping parameters, can be used to online reconfigure the parameters. Example usage:
rosservice call /elevation_mapping/reload_parameters {}
disable_updates ([std_srvs/Empty])
Stops updating the elevation map with sensor input. Trigger the update stopping with
rosservice call /elevation_mapping/disable_updates {}
enable_updates ([std_srvs/Empty])
Start updating the elevation map with sensor input. Trigger the update starting with
rosservice call /elevation_mapping/enable_updates {}
DEPRECATED point_cloud_topic (string, default: "/points")
The name of the distance measurements topic. Use input_sources instead.
**`input_so
暂无开放 Issues，或尚未同步最近议题。