Robot Navigation (3): Navigation related messages

map

There are two main messages related to maps:

nav_msgs/MapMetaData: map metadata, including map width, height, resolution, etc.
nav_msgs/OccupancyGrid: Map raster data, generally displayed graphically in rviz.

nav_msgs/MapMetaData

Call rosmsg info nav_msgs/MapMetaData to display the message content as follows:

time map_load_time
float32 resolution #地图分辨率
uint32 width #地图宽度
uint32 height #地图高度
geometry_msgs/Pose origin #地图位姿数据
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

nav_msgs/OccupancyGrid

Call rosmsg info nav_msgs/OccupancyGrid to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
#--- 地图元数据
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
#--- 地图内容数据，数组长度 = width * height
int8[] data

Odometer

The odometer-related message is: nav_msgs/Odometry, calling rosmsg info nav_msgs/Odometry displays the message as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose #里程计位姿
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist #速度
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z    
  # 协方差矩阵
  float64[36] covariance

coordinate transformation

Coordinate transformation related messages are: tf/tfMessage, call rosmsg info tf/tfMessage to display the message content as follows:

geometry_msgs/TransformStamped[] transforms #包含了多个坐标系相对关系数据的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

position

The positioning related message is: geometry_msgs/PoseArray, call rosmsg info geometry_msgs/PoseArray to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose[] poses #预估的点位姿组成的数组
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

Goal point and path planning

The message related to the target point is: move_base_msgs/MoveBaseActionGoal, call rosmsg info move_base_msgs/MoveBaseActionGoal to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose #目标点位姿
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

The message related to path planning is: nav_msgs/Path, call rosmsg info nav_msgs/Path to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses #由一系列点组成的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

lidar

The laser radar related message is: sensor_msgs/LaserScan, call rosmsg info sensor_msgs/LaserScan to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min #起始扫描角度(rad)
float32 angle_max #终止扫描角度(rad)
float32 angle_increment #测量值之间的角距离(rad)
float32 time_increment #测量间隔时间(s)
float32 scan_time #扫描间隔时间(s)
float32 range_min #最小有效距离值(m)
float32 range_max #最大有效距离值(m)
float32[] ranges #一个周期的扫描数据
float32[] intensities #扫描强度数据，如果设备不支持强度数据，该数组为空

camera

Depth camera related messages are: sensor_msgs/Image, sensor_msgs/CompressedImage, sensor_msgs/PointCloud2

sensor_msgs/Image corresponds to general image data, sensor_msgs/CompressedImage corresponds to compressed image data, and sensor_msgs/PointCloud2 corresponds to point cloud data (image data with depth information).

Call rosmsg info sensor_msgs/Image to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #宽度
string encoding #编码格式:RGB、YUV等
uint8 is_bigendian #图像大小端存储模式
uint32 step #一行图像数据的字节数，作为步进参数
uint8[] data #图像数据，长度等于 step * height

Call rosmsg info sensor_msgs/CompressedImage to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string format #压缩编码格式(jpeg、png、bmp)
uint8[] data #压缩后的数据

Call rosmsg info sensor_msgs/PointCloud2 to display the message content as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #宽度
sensor_msgs/PointField[] fields #每个点的数据类型
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian #图像大小端存储模式
uint32 point_step #单点的数据字节步长
uint32 row_step   #一行数据的字节步长
uint8[] data      #存储点云的数组，总长度为 row_step * height
bool is_dense     #是否有无效点

Depth image to laser data

This section introduces a function package in ROS: depthimage_to_laserscan. As the name suggests, this function package can convert depth image information into lidar information. The application scenarios are as follows:

In many SLAM algorithms, it is generally necessary to subscribe to lidar data to build maps, because lidar can perceive the depth information of the surrounding environment, and depth cameras also have the function of sensing depth information, and the initial price of lidar is relatively expensive, then Can a depth camera be used instead of a lidar in sensor selection?

The answer is yes, but the types of messages released by the two are completely different. If you want to replace the sensor, you need to convert the three-dimensional graphics information released by the depth camera into two-dimensional lidar information. This function is Achieved by depthimage_to_laserscan.

Introduction to depthimage_to_laserscan

The principle of depthimage_to_laserscan is relatively simple to realize the conversion between depth image and radar data. Radar data is two-dimensional and planar, and depth image is three-dimensional, which is the vertical superposition of several two-dimensional (horizontal) data. If the three-dimensional data is converted into two Dimensional data, you only need to take a certain layer of the depth map. For better understanding, please see the official example:

Figure 1: Depth camera and external environment (physical picture)

Figure 2: The image information released by the depth camera, the colored line in the figure corresponds to the data to be converted into radar information

Figure 3: It is more intuitive to display Figure 2 as a point cloud. The colored lines in the figure still correspond to the data to be converted into radar information

Figure 4: The result map after conversion (top view)

Advantages and disadvantages
Advantages: the cost of depth cameras is generally lower than that of lidar, which can reduce hardware costs;

Disadvantages: Compared with lidar, the depth camera has a large gap in detection range and accuracy, and the SLAM effect may not be as ideal as lidar.

Installation
Please install it before use, the command is as follows:

sudo apt-get install ros-melodic-depthimage-to-laserscan

depthimage_to_laserscan node description

The core node of the depthimage_to_laserscan function package is: depthimage_to_laserscan. In order to facilitate calling, you need to understand the topics subscribed to, published topics and related parameters of this node.

Subscribed Topic

image(sensor_msgs/Image): input image information.
camera_info(sensor_msgs/CameraInfo): The camera information of the associated image. Normally no remapping is required, as camera_info will subscribe from the same namespace as image.

Published Topic

scan(sensor_msgs/LaserScan): publish converted lidar type data.

parameter

This node has few parameters, only the following ones, and generally needs to be set: output_frame_id.

~scan_height(int, default: 1 pixel)

Sets the number of pixel rows used to generate lidar information.
~scan_time(double, default: 1/30.0Hz (0.033s))

The time interval between two scans.
~range_min(double, default: 0.45m)

The minimum extent to return. Combined with range_max, only the data between range_min and range_max will be obtained.
~range_max(double, default: 10.0m)

The maximum range to return. Combined with range_min, only the data between range_min and range_max will be obtained.
~output_frame_id(str, default: camera_depth_frame)

The ID of the laser information.

Use depthimage_to_laserscan

Write a launch file
Write a launch file and execute it to convert depth information into radar information

<launch>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth/image_raw" />
        <param name="output_frame_id" value="camera"  />
    </node>
</launch>

The subscribed topic needs to be set according to the topic published by the depth camera, and the output_frame_id needs to be consistent with the coordinate system of the depth camera.

Modify the URDF file
After information conversion, the depth camera will also release the radar data. In order not to cause confusion, you can comment out part of the content about the lidar in the xacro file.

Execution
1. Start the gazebo simulation environment, as follows:

2. Start rviz and add related components (image, LaserScan), the results are as follows:

SLAM application

Now that we have implemented and tested the conversion of depth image information into lidar information, the next step is the practical stage, implementing SLAM through depth cameras. The process is as follows:

1. Start the Gazebo simulation environment first;

2. Start the conversion node;

3. Restart the launch file for map drawing;

4. Start the keyboard keyboard control node, which is used to control the robot's motion and mapping;

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

5. Add components in rviz to display the grid map. Finally, you can control the robot movement in gazebo through the keyboard. At the same time, the grid map data released by gmapping can be displayed in rviz. However, as mentioned earlier, due to the accuracy Due to the lack of environmental features and the detection range, the mapping effect may not be ideal, and the map may even shift.