实验二 读取和理解激光雷达数据

1. 体验内容

（1）为rplidar添加USB权限

注：实验室的rplidar A2买得比较早，硬件版本可能与github程序不匹配，出现运行错误，解决方法为到 Windows 环境登录思岚科技官网下载rplidar A2的固件升级程序，连接激光雷达进行固件升级。

将激光雷达连接到电脑上，检查识别到的USB设备

ls -l /dev|grep ttyUSB

若没有出现如下图所示的“rplidar”字样，说明需要添加USB的默认识别权限，遵循接下来的教程进行操作。

sudo gedit /etc/udev/rules.d/rapliar.rules

输入密码打开文件进行编辑，添加以下内容

KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout",  SYMLINK+="rplidar"

在终端输入指令，增加当前用户对串口的默认访问权限：

sudo usermod -a -G dialout <你的用户名>

使UDEV配置生效（使串口的默认访问权限生效，需要重启机器）

sudo service udev reload
sudo service udev restart

重新插拔激光雷达，再次输入指令

ls -l /dev|grep ttyUSB

此时显示出了“rplidar”字样，以“rplidar→ttyUSB1”为例，输入以下指令赋予权限。

sudo chmod 666 /dev/ttyUSB1

参考博客： https://blog.csdn.net/KEVINZHAO124517/article/details/102137333

（2）下载rplidar_ros进行编译

创建工作空间，进入其src文件夹

git clone https://github.com/Slamtec/rplidar_ros.git
cd ..
catkin_make

（3）设置自动映射rplidar端口

更改程序代码中的映射端口号，打开rplidar.launch及node.cpp搜索/dev/ttyUSB0，将数字改为你前面知晓的连接端口号。

（4）执行程序

直接运行view_rplidar.launch文件，其中已经包含对激光雷达的启动。

roslaunch rplidar_ros view_rplidar.launch

可以通过调用rplidar提供的服务/start_motor和/stop_motor来启动和停止激光雷达，然后通过rviz观看激光雷达数据，注意将Fixed Frame改为laser。

（5）激光雷达数据结构

rosmsg info sensor_msgs/LaserScan

返回激光雷达数据结构

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

2. 作业内容

（1）ros编程

只提取雷达数据前方180°的数据，生成一个新的topic：scan_half。
实现这件事需要同时使用到话题的订阅和发布，在rplidar_ros/src中创建scan_half.cpp文件

#include 
#include 
#include 
#include 

using namespace std;

class Laser
{
public:
	Laser(ros::NodeHandle& n);
	
private:
	ros::NodeHandle n_;
	ros::Subscriber laser_sub_;
	ros::Publisher laser_pub_;
	
	void get_laser_callback(const sensor_msgs::LaserScan &laser);
};

Laser::Laser(ros::NodeHandle& n)
{
	n_ = n;
	laser_pub_ = n_.advertise<std_msgs::Float32>("scan_half",1000);
	laser_sub_ = n_.subscribe("/scan", 10, &Laser::get_laser_callback, this);
}


void Laser::get_laser_callback(const sensor_msgs::LaserScan &laser)
{
	int index = int(laser.ranges.size()/2)-1;
	std_msgs::Float32 msg;
	msg.data = laser.ranges[index];
	laser_pub_.publish(msg);
	cout << "ROS激光雷达180°数据(正前方)" << laser.ranges[index] << endl;
    cout <<"------------------------------------------------------" << endl;
}

int main(int argc, char **argv)
{
	ros::init(argc,argv,"laser_half_scan");
	ros::NodeHandle n;
	Laser laser1(n);
	ros::spin();
	return 0;
}

在CMakeList.txt里添加

add_executable(scan_half src/scan_half.cpp ${RPLIDAR_SDK_SRC})
target_link_libraries(scan_half ${catkin_LIBRARIES})

在工作空间使用catkin_make进行编译。
现在可以运行

rosrun rplidar_ros scan_half

查看正在发布的/scan_half话题

rostopic echo /scan_half

如果不显示，检查一下你的激光雷达是否正在工作。

（2）熟悉TF

利用ROS的TF坐标变换系统，给定激光雷达和机器人底盘的坐标关系，求激光雷达数据转换到底盘坐标系后的读数。

ROS的tf系统分为静态和动态，题目所要求的坐标转换使用静态即可，采用右手坐标系，指定向右为y轴正向，垂直纸面向外为x轴正向，在rplidar.launch中添加如下代码实现坐标关系的定义

<node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.0  0.1  0.2  0.0  0.0  1.0  0.0 base_link laser 100"/>

可以使用如下指令查看tf消息的相关信息

# 查看TF树
rosrun rqt_tf_tree rqt_tf_tree
# 监控坐标系数据的发布及来源节点
rosrun tf tf_monitor
# 生成坐标树的pdf
rosrun tf view_frames
# 监控指定坐标系间变换关系，类似与rostopic echo
rosrun tf tf_echo base_link pole_top_1_link

使用rostopic info查看话题/tf和/tf_static的消息类型是tf2_msgs/TFMessage，接着使用rosmsg info tf2_msgs/TFMessage查看消息的具体数据类型

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

可以看到该数据类型是geometry_msgs/TransformStamped形成的数组，而后者主要由三个部分组成：header、child_frame_id和transform。