机器人自主探索系列解析(一)——仿真环境解析
机器人自主探索系列解析(一)——仿真环境解析
最近实验室又开始了新一轮的无人小车自主探索研究，之前已经在仿真环境中测试了自主导航，效果挺不错的。但是由于一直没来得及详细阅读代码。趁这段时间有空，记录以下对代码的阅读情况。
CMU开源了两个自主探索的代码 TASE和FAR_planner。 同时也提供了全套的导航框架，包括状态估计、地形可通行性分析、避障等算法模块，本文主要对仿真环境部分的代码进行分析。
仿真环境代码链接：
https://github.com/HongbiaoZ/autonomous_exploration_development_environment
1. system_real_robot.launch
因为更多的是想适配实验室自己的设备，所以重点关注了代码中关于实车适配，对应的是system_real_robot.launch文件
system_real_robot.launch代码文件:
<launch>
<arg name="cameraOffsetZ" default="0"/>
<arg name="vehicleX" default="0"/>
<arg name="vehicleY" default="0"/>
<arg name="checkTerrainConn" default="false"/>
<!-- <include file="$(find lio_sam)/launch/run.launch" /> -->
<include file="$(find fast_lio)/launch/mapping_velodyne.launch" />
<include file="$(find ps3joy)/launch/ps3.launch" />
<include file="$(find local_planner)/launch/local_planner.launch" >
<arg name="cameraOffsetZ" value="$(arg cameraOffsetZ)"/>
<arg name="goalX" value="$(arg vehicleX)"/>
<arg name="goalY" value="$(arg vehicleY)"/>
</include>
<include file="$(find terrain_analysis)/launch/terrain_analysis.launch" />
<include file="$(find terrain_analysis_ext)/launch/terrain_analysis_ext.launch" >
<arg name="checkTerrainConn" value="$(arg checkTerrainConn)"/>
</include>
<include file="$(find sensor_scan_generation)/launch/sensor_scan_generation.launch" />
<include file="$(find loam_interface)/launch/loam_interface.launch" />
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rvizGA" args="-d $(find vehicle_simulator)/rviz/vehicle_simulator.rviz" respawn="true"/>
</launch>
实车配置和仿真环境不同在于，仿真中里程计使用的是gazebo得到的真实位姿，且点云是直接发布在
map全局坐标系下的。在适配到自己设备时，需要外部的SLAM算法提供对应的里程计 /topic和点云地图 /topic
2.loam_interface
launch文件
loam_interface提供了一个和slam状态估计节点通讯的接口，可以很方便的在launch文件中通过修改slam算法的topic，适配不同类型的slam算法 实例。
对应的launch文件如下：
<launch>
<node pkg="loam_interface" type="loamInterface" name="loamInterface" output="screen" required="true">
<!-- <param name="stateEstimationTopic" type="string" value="/lio_sam/mapping/odometry" />
<param name="registeredScanTopic" type="string" value="/lio_sam/mapping/cloud_registered" /> -->
<param name="stateEstimationTopic" type="string" value="/Odometry" />
<param name="registeredScanTopic" type="string" value="/cloud_registered" />
<param name="flipStateEstimation" type="bool" value="false" />
<param name="flipRegisteredScan" type="bool" value="false" />
<param name="sendTF" type="bool" value="true" />
<param name="reverseTF" type="bool" value="false" />
</node>
</launch>
其中stateEstimationTopic和stateEstimationTopic需要替换为实际SLAM发布的话题。后续一些参数是tf变换的一些操作，例如翻转点云、翻转位姿等。
本文以lio-sam为状态估计节点
ros::Subscriber subOdometry = nh.subscribe<nav_msgs::Odometry> (stateEstimationTopic, 5, odometryHandler);
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> (registeredScanTopic, 5, laserCloudHandler);
odometryHandler
里程计回调函数。订阅里程计信息，liosam中是/lio_sam/mapping/odometry(机器人在世界坐标系下的位姿)，执行回调函数odometryHandler(将机器人在世界坐标系下的位姿换个话题名/state_estimation发布，同时发布TF变换)
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odom)
{
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = odom->pose.pose.orientation;
odomData = *odom;
if (flipStateEstimation) { // flip the odometry data 将原始ROS下的ENU坐标系转换为LOAM下的ENU坐标系 X’ = Z, Y’ = X, Z’ = Y
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
pitch = -pitch;
yaw = -yaw;
geoQuat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
odomData.pose.pose.orientation = geoQuat;
odomData.pose.pose.position.x = odom->pose.pose.position.z;
odomData.pose.pose.position.y = odom->pose.pose.position.x;
odomData.pose.pose.position.z = odom->pose.pose.position.y;
}
// publish odometry messages
odomData.header.frame_id = "map";
odomData.child_frame_id = "sensor";
pubOdometryPointer->publish(odomData);
// publish tf messages
odomTrans.stamp_ = odom->header.stamp;
odomTrans.frame_id_ = "map";
odomTrans.child_frame_id_ = "sensor";
odomTrans.setRotation(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w));
odomTrans.setOrigin(tf::Vector3(odomData.pose.pose.position.x, odomData.pose.pose.position.y, odomData.pose.pose.position.z));
if (sendTF) {
if (!reverseTF) {
tfBroadcasterPointer->sendTransform(odomTrans);
} else {
tfBroadcasterPointer->sendTransform(tf::StampedTransform(odomTrans.inverse(), odom->header.stamp, "sensor", "map"));
}
}
}
laserCloudHandler
点云回调函数。订阅点云信息,liosam中是/lio_sam/mapping/cloud_registered(世界坐标系下的关键帧),执行回调函数laserCloudHandler,将世界坐标系下的关键帧点云换个话题名/registered_scan重新发布出去
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn)
{
laserCloud->clear();
pcl::fromROSMsg(*laserCloudIn, *laserCloud);
if (flipRegisteredScan) { // flip the point cloud data 将原始ROS下的ENU坐标系转换为LOAM下的ENU坐标系 X’ = Z, Y’ = X, Z’ = Y
int laserCloudSize = laserCloud->points.size();
for (int i = 0; i < laserCloudSize; i++) {
float temp = laserCloud->points[i].x;
laserCloud->points[i].x = laserCloud->points[i].z;
laserCloud->points[i].z = laserCloud->points[i].y;
laserCloud->points[i].y = temp;
}
}
// publish registered scan messages
sensor_msgs::PointCloud2 laserCloud2;
pcl::toROSMsg(*laserCloud, laserCloud2);
laserCloud2.header.stamp = laserCloudIn->header.stamp;
laserCloud2.header.frame_id = "map";
pubLaserCloudPointer->publish(laserCloud2);
}
总的来说 loam_interface提供了一个slam接口，不断接收slam部分提供的世界坐标系下的当前机器人的位姿/Odometry和关键帧点云/cloud。并以新的话题/state_estimation和/registered_scan话题名发布出去。
3.sensor_scan_generation
这部分的内容主要是提供了一个坐标转换的功能：将世界坐标系下的点云转换到传感器坐标下
即订阅/state_estimation和/registered_scan 话题，发布/state_estimation_at_scan和/sensor_scan话题。
主要代码如下：
void laserCloudAndOdometryHandler(const nav_msgs::Odometry::ConstPtr& odometry,
const sensor_msgs::PointCloud2ConstPtr& laserCloud2)
{
laserCloudIn->clear();
laserCLoudInSensorFrame->clear();
pcl::fromROSMsg(*laserCloud2, *laserCloudIn);//将ROS消息转换为PCL点云
odometryIn = *odometry;//接收里程计数据
// 创建一个transformToMap变换， 表示传感器坐标系在地图坐标系下的位置和方向
transformToMap.setOrigin(
tf::Vector3(odometryIn.pose.pose.position.x, odometryIn.pose.pose.position.y, odometryIn.pose.pose.position.z));
transformToMap.setRotation(tf::Quaternion(odometryIn.pose.pose.orientation.x, odometryIn.pose.pose.orientation.y,odometryIn.pose.pose.orientation.z, odometryIn.pose.pose.orientation.w));
int laserCloudInNum = laserCloudIn->points.size();
pcl::PointXYZ p1;
tf::Vector3 vec;
/*将点云数据从地图坐标系转换到传感器坐标系。
*对于每个点，使用transformToMap变换将其
*从地图坐标系变换到传感器坐标系，然后将变换
*后的点云数据存储在 laserCloudInSensorFrame中。
*/
for (int i = 0; i < laserCloudInNum; i++)
{
p1 = laserCloudIn->points[i];
vec.setX(p1.x);
vec.setY(p1.y);
vec.setZ(p1.z);
vec = transformToMap.inverse() * vec;// 将点从地图坐标系转换到传感器坐标系
//p_a=T_ab*p_b --> p_b=(T_ab)-1 *p_a
p1.x = vec.x();
p1.y = vec.y();
p1.z = vec.z();
laserCLoudInSensorFrame->points.push_back(p1);
}
//发布里程计信息 map到sensor_at_scan的odom
odometryIn.header.stamp = laserCloud2->header.stamp;
odometryIn.header.frame_id = "map";
odometryIn.child_frame_id = "sensor_at_scan";
pubOdometryPointer->publish(odometryIn);
//发布tf变换
transformToMap.stamp_ = laserCloud2->header.stamp;
transformToMap.frame_id_ = "map";
transformToMap.child_frame_id_ = "sensor_at_scan";
tfBroadcasterPointer->sendTransform(transformToMap);
//发布转换后的在传感器坐标下的点云
sensor_msgs::PointCloud2 scan_data;
pcl::toROSMsg(*laserCLoudInSensorFrame, scan_data);
scan_data.header.stamp = laserCloud2->header.stamp;
scan_data.header.frame_id = "sensor_at_scan";
pubLaserCloud.publish(scan_data);
}
总的来说 sensor_scan_generation 提供了一个坐标转换功能，作用如下：
- 订阅/state_estimation和/registered_scan
- 发布/state_estimation_at_scan(等同于/state_estimation)
- 发布/sensor_scan (将世界坐标系下的点云转换到传感器坐标系下)
之所以要进行坐标变换，是因为我们在导航、规划过程中，往往希望点云是在传感器坐标系，而不是世界坐标系下。这部分在后续的pathFollwer中介绍。
4.总结
在这篇文章中，我们主要介绍了机器人自主探索中的仿真环境部分，重点介绍了几个接口内容。例如loam_interface和sensor_scan_generation。这些接口很好的适配了不同的SLAM接口和导航接口。 接口的作用非常重要，直接会影响到仿真以及实车的适配性。
仿真环境和实车调试的区别主要在：
- 机器人在全局坐标系的位姿获取不同，仿真来源于gazebo提供的真实位姿，实车则需要SLAM提供对应位姿
- 仿真中，所有的点云都是在/map坐标系下，而实车中则希望是在车辆坐标系base_link或者传感器坐标系上。因此代码提供了很多从全局坐标转换到车辆坐标系的操作。例如点云裁剪、距离判断等。
- loam_interface提供了slam的接口，sensor_scan_generation将世界坐标系下的点云转换到传感器坐标上
浙公网安备 33010602011771号