一、子函数分析

1、发布数据子函数

(1)雷达数据数据类型

Header header            # timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad] float32 time_increment # time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
float32 scan_time # time between scans [seconds] float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m] float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities # intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty.

(2)雷达发布子函数分析

/***********************打印函数*************************/
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) //id
{
static int scan_count = ;
sensor_msgs::LaserScan scan_msg; //ros已有的雷达数据模型 scan_msg.header.stamp = start; //扫描起始时间
scan_msg.header.frame_id = frame_id; //序列id
scan_count++; //计数 //角度修正,从小到大
bool reversed = (angle_max > angle_min);
if ( reversed )
{
scan_msg.angle_min = M_PI - angle_max;
scan_msg.angle_max = M_PI - angle_min;
} else
{
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 = 8.0; //最大的范围 scan_msg.intensities.resize(node_count); //信号质量
scan_msg.ranges.resize(node_count); //范围 bool reverse_data = (!inverted && reversed) || (inverted && !reversed);
if (!reverse_data)
{
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);
}

2、得到设备信息子函数

/*****************得到设备信息**************************/
bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
{
u_result op_result;
rplidar_response_device_info_t devinfo; //得到设备信息
op_result = drv->getDeviceInfo(devinfo); //失败信息
if (IS_FAIL(op_result))
{
if (op_result == RESULT_OPERATION_TIMEOUT)
{
fprintf(stderr, "Error, operation time out.\n");
} else
{
fprintf(stderr, "Error, unexpected error, code: %x\n", op_result);
}
return false;
} // print out the device serial number, firmware and hardware version number..序列号
printf("RPLIDAR S/N: ");
for (int pos = ; pos < ;++pos)
{
printf("%02X", devinfo.serialnum[pos]);
}
//版本
printf("\n"
"Firmware Ver: %d.%02d\n"
"Hardware Rev: %d\n"
, devinfo.firmware_version>>
, devinfo.firmware_version & 0xFF
, (int)devinfo.hardware_version);
return true;
}

3、检查rplidar设备健康信息

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;
}
}

4、雷达开始/结束

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;
}

5、main函数分析

int main(int argc, char * argv[])
{
ros::init(argc, argv, "rplidar_node"); std::string serial_port; //串口号
int serial_baudrate = ; //串口波特率
std::string frame_id; //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("~"); //launch可以进行一些初始化
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); printf("RPLIDAR running on ROS package rplidar_ros\n"
"SDK Version: "RPLIDAR_SDK_VERSION"\n"); u_result op_result; // create the driver instance,创建静态接口
drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT); if (!drv) //创建失败退出-2
{
fprintf(stderr, "Create Driver fail, exit\n");
return -;
} // make connection...建立链接,失败返回-1
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 -;
} // get rplidar device info 获得设备信息
if (!getRPLIDARDeviceInfo(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); //开电机进行扫描
drv->startMotor();
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 = ; //初始化,清0
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 != ) //距离不是0
{
//计算当前角度,如果角度比上次小则,记录
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!
drv->stop();
drv->stopMotor();
RPlidarDriver::DisposeDriver(drv);
return ;
}

二、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>

1、运行launch文件

(1)格式:

roslaunch package_name launch_file_name
 Tip1: rosrun只能运行一个nodes, roslaunch可以同时运行多个nodes

(2)详细显示(request verbosity)

(3) 结束launch文件

ctrl+c

2、创建launch文件

(1) launch文件一般以.launch后缀作为文件名,放在package的launch文件夹下。最简单的launch文件可以仅包含几个nodes。

(2) Launch文件是XML文件,每个XML文件必须有一个root element。而launch文件的root element由一对launch 标签定义。

<launch>

...

</launch>

(3) launch文件的核心是一系列node elements,每个node element启动一个node。Node element看起来如下

<node

  pkg=”package_name” type=”executable_name” name=”node_name”

/>

#Tip1: 最后的“/”是必不可少的。

#Tip2: 也可以写成<node pkg=”..” type=”...” name=”...”></node>

#如果该node中有其他tags,则必须使用这种形式。

(4) 一个node element包含三个必须的属性:pkg, type, name.

  pkg和type属性指出ROS应该运行哪个pkg中的哪个node,注意:此处的type是可执行文件的名称,而name则是可以任意给出的,它覆盖了原有文件中ros::init指定的node name。

(5) node日志文件(log file)

  运行roslaunch和rosrun运行单个节点的区别之一是,默认情况下,roslaunch运行的nodes的标准输出会重定向到log file,而不是控制台

~/.ros/log/run_id/node_name-number-stdout.log

(6) 输出到控制台

  用output属性, output=”screen”;这种方法仅显示一个node。

  若显示所有nodes的输出,用--screen命令行。

roslaunch --screen package_name launch_file_name

  如果正在运行的文件没有显示想要对输出,可以查看该node属性集中是否有 output=”screen”.

(7) 在独立的窗口运行各nodes

  我们在各自的termin运行rosrun node_name;但是运行roslaunch时,所有的nodes共用一个相同的terminal,这对于那些需要从控制台输入的nodes很不方便。可以使用launch-prefix属性。

launch-prefix=”command-prefix”

Eg:launch-prefix=”xterm -e” 等价于  xterm -e rosrun turtlesim turtle_teleop_key

  xterm 命令表示新建一个terminal; -e参数告诉xterm执行剩下的命令行。

  当然,launch-prefix属性不仅仅限于xterm。它可用于调试(通过gdb或valgrind),或用于降低进程的执行顺序(通过nice).

Rplidar学习(四)—— ROS下进行rplidar雷达数据采集源码分析的更多相关文章

  1. Springboot学习04-默认错误页面加载机制源码分析

    Springboot学习04-默认错误页面加载机制源码分析 前沿 希望通过本文的学习,对错误页面的加载机制有这更神的理解 正文 1-Springboot错误页面展示 2-Springboot默认错误处 ...

  2. # Volley源码解析(二) 没有缓存的情况下直接走网络请求源码分析#

    Volley源码解析(二) 没有缓存的情况下直接走网络请求源码分析 Volley源码一共40多个类和接口.除去一些工具类的实现,核心代码只有20多个类.所以相对来说分析起来没有那么吃力.但是要想分析透 ...

  3. 设计模式(二十四)——职责链模式(SpringMVC源码分析)

    1 学校 OA 系统的采购审批项目:需求是 采购员采购教学器材 1) 如果金额 小于等于 5000,  由教学主任审批 (0<=x<=5000) 2) 如果金额 小于等于 10000,   ...

  4. 转!!Java学习之自动装箱和自动拆箱源码分析

    自动装箱(boxing)和自动拆箱(unboxing)   首先了解下Java的四类八种基本数据类型   基本类型 占用空间(Byte) 表示范围 包装器类型 boolean 1/8 true|fal ...

  5. Java学习之自动装箱和自动拆箱源码分析

    自动装箱(boxing)和自动拆箱(unboxing) 首先了解下Java的四类八种基本数据类型   基本类型 占用空间(Byte) 表示范围 包装器类型 boolean 1/8 true|false ...

  6. Java并发包源码学习之线程池(一)ThreadPoolExecutor源码分析

    Java中使用线程池技术一般都是使用Executors这个工厂类,它提供了非常简单方法来创建各种类型的线程池: public static ExecutorService newFixedThread ...

  7. Future模式的学习以及JDK内置Future模式的源码分析

    并发程序设计之Future模式 一).使用Future模式的原因 当某一段程序提交了一个请求,期待得到一个答复,但服务程序对这个请求的处理可能很慢,在单线程的环境中,调用函数是同步的,必须等到服务程序 ...

  8. 一步步搭建自己的轻量级MVCphp框架-(四)一个国产轻量级框架Amysql源码分析(3) 总进程对象

    AmysqlProcess类,框架的总进程对象 ./Amysql/Amysql.php 下面还是和以前一样,先上代码~ class AmysqlProcess { public $AmysqlCont ...

  9. Java并发包源码学习之AQS框架(四)AbstractQueuedSynchronizer源码分析

    经过前面几篇文章的铺垫,今天我们终于要看看AQS的庐山真面目了,建议第一次看AbstractQueuedSynchronizer 类源码的朋友可以先看下我前面几篇文章: <Java并发包源码学习 ...

随机推荐

  1. 华为交换机SNMP OID

    http://vbb.fyjy.net:88/showthread.php?t=4647

  2. Redis数据结构之list

    一:介绍 1.存储list ArrayList使用数组的方式 LinkedList使用双向链接的方式 二:Redis客户端 1.左端与右端插入 2.左端查询 3.左端与右端弹出 4.长度 5.在头部插 ...

  3. (转)获取 request 中用POST方式"Content-type"是"application/x-www-form-urlencoded;charset=utf-8"发送的 json 数据

    request中发送json数据用post方式发送Content-type用application/json;charset=utf-8方式发送的话,直接用springMVC的@RequestBody ...

  4. macbook安装并破解Clion2018(Pycharm也一样)

    一.下载 Clion 并安装 二.修改hosts文件,使用 vim /private/etc/hosts 命令将 0.0.0.0 account.jetbrains.com 粘贴进去(提示: i键是插 ...

  5. A+B Problem(再升级)

    洛谷P1832 A+B Problem(再升级) ·给定一个正整数n,求将其分解成若干个素数之和的方案总数. 先说我的垃圾思路,根本没有验证它的正确性就xjb写的,过了垃圾样例,还水了20分,笑哭.. ...

  6. cordova 5.4版本 适配全面屏 底部黑边问题

    在全面屏发布之后,Android官方提供了适配方案,即提高App所支持的最大屏幕纵横比,实现很简单,在AndroidManifest.xml中可做如下配置: 更改android.max_aspect值 ...

  7. Linux服务器下Nginx与Apache共存

    解决思路: 将nginx作为代理服务器和web服务器使用,nginx监听80端口,Apache监听除80以外的端口,我这暂时使用8080端口. nginx.conf 位置:/etc/nginx/ngi ...

  8. Prism中命令可用性无法自动刷新

    http://stackoverflow.com/questions/2444927/wpf-prism-canexecute-method-not-being-called It is most l ...

  9. Android studio 3.0以上版本无法引入,找不到v4,v7包方案解决

    Android studio 3.0以上版本无法引入v4.v7包报红,即找不到v4.v7包,解决方案如下: 步骤: 1,Close Object,点击左上角的关闭工程. 2,点击x,删除项目. 3,重 ...

  10. FMDB使用简介

    转:http://my.oschina.net/youzaiyouzaie/blog/92325 源码地址:https://github.com/ccgus/fmdb 这次要分享的是在iOS中使用SQ ...