原创博客:转载请表明出处:http://www.cnblogs.com/zxouxuewei/
由于市面上买的激光雷达价格太贵了。所以在学习时会造成很大的经济压力。但是最近好多做机器人核心组件的公司都开发有廉价的雷达；
本博客教你如何 使用rplidar发布scan数据，在rviz视图中查看scan数据。
1.首先确保你的rplidar水平放置在你的机器人上：(注意安装方向)
2.然后下载rplidar的rosSDK开发包，
3.初步测试：
运行roscore
roscore ＆
直接运行rplidar的launch文件
roslaunch rplidar_ros view_rplidar.launch
查看view_rplidar.launch文件
<!--
Used for visualising rplidar in action. It requires rplidar.launch.
-->
<launch>
<include file="$(find rplidar_ros)/launch/rplidar.launch" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
</launch>
主要是查看rplidar.launch文件
<launch>
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/ttyUSB0"/>
<param name="serial_baudrate" type="int" value=""/>
<param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
</launch>
在这个launch文件中打开了rplidarNode这个节点，然后我们分析一下节点源码：
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_srvs/Empty.h" #include "rplidar.h" //RPLIDAR standard sdk, all-in-one header #ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif #define DEG2RAD(x) ((x)*M_PI/180.) using namespace rp::standalone::rplidar; RPlidarDriver * drv = NULL; void publish_scan(ros::Publisher *pub,
rplidar_response_measurement_node_t *nodes,
size_t node_count, ros::Time start,
double scan_time, bool inverted,
float angle_min, float angle_max,
std::string frame_id)
{
static int scan_count = ;
sensor_msgs::LaserScan scan_msg; scan_msg.header.stamp = start;
scan_msg.header.frame_id = frame_id;
scan_count++; scan_msg.angle_min = M_PI - angle_min;
scan_msg.angle_max = M_PI - angle_max;
scan_msg.angle_increment =
(scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-); scan_msg.scan_time = scan_time;
scan_msg.time_increment = scan_time / (double)(node_count-); scan_msg.range_min = 0.15;
scan_msg.range_max = .; scan_msg.intensities.resize(node_count);
scan_msg.ranges.resize(node_count);
if (!inverted) { // assumes scan window at the top
for (size_t i = ; i < node_count; i++) {
float read_value = (float) nodes[i].distance_q2/4.0f/;
if (read_value == 0.0)
scan_msg.ranges[i] = std::numeric_limits<float>::infinity();
else
scan_msg.ranges[i] = read_value;
scan_msg.intensities[i] = (float) (nodes[i].sync_quality >> );
}
} else {
for (size_t i = ; i < node_count; i++) {
float read_value = (float)nodes[i].distance_q2/4.0f/;
if (read_value == 0.0)
scan_msg.ranges[node_count--i] = std::numeric_limits<float>::infinity();
else
scan_msg.ranges[node_count--i] = read_value;
scan_msg.intensities[node_count--i] = (float) (nodes[i].sync_quality >> );
}
} pub->publish(scan_msg);
} bool checkRPLIDARHealth(RPlidarDriver * drv)
{
u_result op_result;
rplidar_response_device_health_t healthinfo; op_result = drv->getHealth(healthinfo);
if (IS_OK(op_result)) {
printf("RPLidar health status : %d\n", healthinfo.status); if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
fprintf(stderr, "Error, rplidar internal error detected."
"Please reboot the device to retry.\n");
return false;
} else {
return true;
} } else {
fprintf(stderr, "Error, cannot retrieve rplidar health code: %x\n",
op_result);
return false;
}
} bool stop_motor(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
if(!drv)
return false; ROS_DEBUG("Stop motor");
drv->stop();
drv->stopMotor();
return true;
} bool start_motor(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
if(!drv)
return false;
ROS_DEBUG("Start motor");
drv->startMotor();
drv->startScan();;
return true;
} int main(int argc, char * argv[]) {
ros::init(argc, argv, "rplidar_node"); std::string serial_port;
int serial_baudrate = ;
std::string frame_id;
bool inverted = false;
bool angle_compensate = true; ros::NodeHandle nh;
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", );
ros::NodeHandle nh_private("~");
nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
nh_private.param<int>("serial_baudrate", serial_baudrate, );
nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
nh_private.param<bool>("inverted", inverted, "false");
nh_private.param<bool>("angle_compensate", angle_compensate, "true"); u_result op_result; // create the driver instance
drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT); if (!drv) {
fprintf(stderr, "Create Driver fail, exit\n");
return -;
} // make connection...
if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
, serial_port.c_str());
RPlidarDriver::DisposeDriver(drv);
return -;
} // check health...
if (!checkRPLIDARHealth(drv)) {
RPlidarDriver::DisposeDriver(drv);
return -;
} ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor);
ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor); // start scan...
drv->startScan(); ros::Time start_scan_time;
ros::Time end_scan_time;
double scan_duration;
while (ros::ok()) { rplidar_response_measurement_node_t nodes[*];
size_t count = _countof(nodes); start_scan_time = ros::Time::now();
op_result = drv->grabScanData(nodes, count);
end_scan_time = ros::Time::now();
scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-; if (op_result == RESULT_OK) {
op_result = drv->ascendScanData(nodes, count); float angle_min = DEG2RAD(0.0f);
float angle_max = DEG2RAD(359.0f);
if (op_result == RESULT_OK) {
if (angle_compensate) {
const int angle_compensate_nodes_count = ;
const int angle_compensate_multiple = ;
int angle_compensate_offset = ;
rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
memset(angle_compensate_nodes, , angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
int i = , j = ;
for( ; i < count; i++ ) {
if (nodes[i].distance_q2 != ) {
float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
int angle_value = (int)(angle * angle_compensate_multiple);
if ((angle_value - angle_compensate_offset) < ) angle_compensate_offset = angle_value;
for (j = ; j < angle_compensate_multiple; j++) {
angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
}
}
} publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
start_scan_time, scan_duration, inverted,
angle_min, angle_max,
frame_id);
} else {
int start_node = , end_node = ;
int i = ;
// find the first valid node and last valid node
while (nodes[i++].distance_q2 == );
start_node = i-;
i = count -;
while (nodes[i--].distance_q2 == );
end_node = i+; angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +,
start_scan_time, scan_duration, inverted,
angle_min, angle_max,
frame_id);
}
} else if (op_result == RESULT_OPERATION_FAIL) {
// All the data is invalid, just publish them
float angle_min = DEG2RAD(0.0f);
float angle_max = DEG2RAD(359.0f); publish_scan(&scan_pub, nodes, count,
start_scan_time, scan_duration, inverted,
angle_min, angle_max,
frame_id);
}
} ros::spinOnce();
} // done!
RPlidarDriver::DisposeDriver(drv);
return ;
}
主要是打开串口读取rplidar的数据，然后发布scan话题。
查看rqt_graph节点视图。