> 說明:
> 本文首發於 Playfish Blog，轉載請保留鏈接。
前言
在上一篇博客中，介紹瞭如何在ROS1下編寫激光測距傳感器節點，在此基礎上我們下面講述如何在ROS2下編程來創建激光測距傳感器節點。
在接下來的博客中將根據本身經驗來編寫一些ROS相關內容，權當是作爲記憶來分享。
ROS2驅動——點激光掃描儀
下面將講述使用ROS官方未有的傳感器來作爲樣例，我使用的是萊旭光電的CHT-10點激光傳感器作爲 介紹，首先這款傳感器通信協議部分比較簡單，基本上打開串口就可以讀出數據，在將數據進行轉換就可以得到想要的距離，比較方便，免去複雜的部分，對教程也有幫助，畢竟教程主要講述的是ROS2驅動部分。
爲了不妨礙與上一篇的銜接問題，這裏我們重新開始從頭介紹，大家就可以不用看上一篇博客。
相關ROS2驅動代碼可以在github上找到進行下載：https://github.com/Playfish/cht10_node/tree/ros2
CHT-10是點激光，掃描範圍是0.05-10m，真實的應該是0.15-10m，10m未測試，不過0.15盲區還是有的。
下圖是CHT-10的通信協議部分介紹：
可以看到想要的數據在9~12位（四字節），得到的數據需要除以1000才能得到真實的距離（米），ROS消息中基本單位是米。
### 消息選定
關於選擇什麼消息作爲該傳感器的ROS2消息，這個相對來說不是很重要，不過作爲通用項來說，儘量向通用靠近（即使用ROS官方消息定義，而不是自己定義消息）。目前激光消息的話，有兩個選擇，第一個是sensor_msgs/LaserScan消息類型，不過該消息類型適合有角度的傳感器，即180°或360°的2D雷達或激光，而目前使用的激光是單點類型，和激光筆差不多，只有一個點，那麼可以選擇sensor_msgs/Range消息類型，該消息類型適用於超聲波傳感器、紅外傳感器單點類型，CHT-10就用這個類型。
注意：在ROS2中，頭文件將採用<消息包>/<通信機制>/消息名稱.hpp形式，拿本教程爲例，
| ROS1 | ROS2 |
| sensor_msgs/Range.h | sensor_msgs/msg/range.hpp |
關於該ROS2消息,官方暫未提供API文檔，大家可以查看該文件來查看API，基本語法與ROS1一致。
準備工作
傳感器選型選定、通信消息選定，那麼接下來還需要做：
- Frame_id：Frame在ROS1中作用至關重要，消息將和TF綁定纔可以讀取數據，在這裏作爲通用可配置，暫定內容爲：laser，用戶可自定義設置（通過ROS2 Parameters設置）。
- 串口：避免和其他傳感器串口衝突，因此在這裏預留出一個串口設置參數，用戶可以自定義設置（通過ROS2 Parameters設置）。
- 話題：消息內容需要通過話題發佈，並且話題需要唯一，不然容易崩潰，在這裏選擇話題爲“range”
當然以上部分可以不考慮，這個是作爲通用型需要考慮的，暫時可以先忘記以上部分，下面開啓代碼部分
測試代碼編寫
在這部分，主要先編寫測試代碼，如果測試代碼無問題，就可以寫到node中，測試代碼和ROS無關係，只是通過串口讀取數據，並且在終端顯示出來，整體思路如下：
傳感器選擇 -> 測試程序測試（類似串口助手） -> ROS2 node綁定（將測試程序讀取部分加入到ROS2中）
下面是測試程序部分，也可以在cht10_node/test中找到，名爲test_cht10.cpp：
#include <string>
#include <cht10_node/serial_func.hpp>
#include <cstdlib>
#include <iostream>
#include <stdint.h>
#define BUFSIZE 17
int main(int argc, char** argv){
cht10_serial_func::Cht10Driver cht10driver_;
std::string serialNumber_;
serialNumber_ = "/dev/ttyUSB0";
int baudRate_ = 115200;
std::stringstream ostream;
int fd, len, rcv_cnt;
bool success_flag;
char buf[40], temp_buf[BUFSIZE],result_buf[BUFSIZE];
unsigned int laser_data=0;
char data_buf[4];
rcv_cnt = 0;
success_flag = false;
memset(buf, 0xba, sizeof(buf));
memset(temp_buf, 0xba, sizeof(temp_buf));
memset(result_buf, 0xba, sizeof(result_buf));
fd = open(serialNumber_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );
if(fd < 0){
std::cout<<"Open Serial: "<<serialNumber_.c_str()<<" Error!";
exit(0);
}
cht10driver_.UART0_Init(fd,baudRate_,0,8,1,'N');
while(1){
len = cht10driver_.UART0_Recv(fd, buf,40);
if(len>0){
for(int i = 0; i < len; i++){
if(rcv_cnt<=BUFSIZE){
result_buf[rcv_cnt++] = buf[i];
if(rcv_cnt == BUFSIZE){
success_flag = true;
}
}//end if
else{
/****
* checkout received data
*/
success_flag = false;
for(int count = 0; count < 4; count++){
data_buf[count] = result_buf[9+count];
}
sscanf(data_buf, "%x", &laser_data);
std::cout<<"sensor data:"<<laser_data<<", Distance: "<<(double)laser_data/1000<<std::endl;
/****
* data writing end
*/
if('~' == buf[i]){
rcv_cnt = 0;
result_buf[rcv_cnt++] = buf[i];
}
}//end else
}//end for
}
}
}
我將串口部分封裝成一個類名爲Cht10Driver，該類包含串口讀/寫部分、初始化。
通過使用ament build可以得到一個名爲test_cht10的可執行文件，使用./test_cht10可以運行，將CHT-10插入到電腦上，並且電腦只有一個ttyUSB0，就可以讀取數據，數據將顯示爲sensor data: 傳感器毫米數據， Distance: 傳感器米數據。
ROS2驅動編寫
上部分講述了測試程序編寫，主要作用是通過串口讀取傳感器數據，那麼得到傳感器數據之後，就可以將傳感器數據填充到ROS消息中，然後通過話題形式發佈出去，如下是將傳感器數據填充到ROS消息並生成節點的主要部分：
完整代碼可以查看： cht10_node.cpp
/**
* @file /cht10_node/src/cht10/cht10_node.cpp
*
* @brief Implementation for dirver with read data from Cht10 nodelet
*
* @author Carl
*
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include <chrono>
#include <cstdio>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <rcutils/logging_macros.h>
#include <functional>
#include <std_msgs/msg/string.hpp>
#include <sensor_msgs/msg/range.hpp>
#include <cht10_node/cht10_node.h>
#define ROS_ERROR RCUTILS_LOG_ERROR
#define ROS_INFO RCUTILS_LOG_INFO
#define ROS_ERROR_THROTTLE(sec, ...) RCUTILS_LOG_ERROR_THROTTLE(RCUTILS_STEADY_TIME, sec, __VA_ARGS__)
using namespace cht10_serial_func;
Cht10Func::Cht10Func(rclcpp::Node::SharedPtr & node):node_(node),
serialNumber_("/dev/USB0"),
frame_id("laser"){
msg_ = std::make_shared<sensor_msgs::msg::Range>();
scan_pub_ = node_->create_publisher<sensor_msgs::msg::Range>("range");
parameter_service_ = std::make_shared<rclcpp::ParameterService>(node_);
node_->get_parameter("serialNumber", serialNumber_);
node_->get_parameter("baudRate", baudRate_);
node_->get_parameter("frame_id", frame_id);
rcv_cnt = 0;
success_flag = 0;
fd = open(serialNumber_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );
if(fd < 0){
ROS_ERROR("Open Serial: %s Error!",serialNumber_.c_str());
exit(0);
}
memset(buf, 0, sizeof(buf));
memset(temp_buf, 0, sizeof(temp_buf));
memset(result_buf, 0, sizeof(result_buf));
Cht10driver_.UART0_Init(fd,baudRate_,0,8,1,'N');
ROS_INFO("Open serial: [ %s ], successfully, with idex: %d.", serialNumber_.c_str() ,fd);
update();
}
Cht10Func::~Cht10Func(){
}
double Cht10Func::data_to_meters(unsigned int &data, int scale){
return (double)data/scale;
}
void Cht10Func::publish_scan(double nodes, builtin_interfaces::msg::Time start,
std::string frame_id){
float final_range;
std::shared_ptr<sensor_msgs::msg::Range> range_msg;
range_msg->field_of_view = 0.05235988;
range_msg->max_range = 10.0;
range_msg->min_range = 0.05;
range_msg->header.frame_id = frame_id;
range_msg->radiation_type = sensor_msgs::msg::Range::INFRARED;
if(nodes > range_msg->max_range){
final_range = std::numeric_limits<float>::infinity();
}else if(nodes < range_msg->min_range){
final_range = -std::numeric_limits<float>::infinity();
}else{
final_range = nodes;
}
range_msg->header.stamp = start;
range_msg->range = final_range;
scan_pub_->publish(range_msg);
}
bool Cht10Func::get_scan_data(){
len = Cht10driver_.UART0_Recv(fd, buf,40);
if(len>0){
for(int i = 0; i < len; i++){
if(rcv_cnt<=BUFSIZE){
result_buf[rcv_cnt++] = buf[i];
if(rcv_cnt == BUFSIZE){
success_flag = true;
}
}//end if
else{
/****
* checkout received data
*/
success_flag = false;
for(int count = 0; count < 4; count++){
data_buf[count] = result_buf[9+count];
}
sscanf(data_buf, "%x", &laser_data);
//std::cout<<"sensor data:"<<laser_data<<std::endl;
/****
* data writing end
*/
if('~' == buf[i]){
rcv_cnt = 0;
result_buf[rcv_cnt++] = buf[i];
}
}//end else
}//end for
}
return success_flag;
}
void Cht10Func::update(){
rcv_cnt = 0;
success_flag = 0;
laser_data = 0;
memset(buf, 0, sizeof(buf));
memset(temp_buf, 0, sizeof(temp_buf));
memset(result_buf, 0, sizeof(result_buf));
ROS_INFO("Begin receive data from %s, with idex: %d.",serialNumber_.c_str(),fd);
fd = open(serialNumber_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );
Cht10driver_.UART0_Init(fd,baudRate_,0,8,1,'N');
// Create a function for when messages are to be sent.
auto publish_message =
[this]() -> void
{
// start_scan_time =
rclcpp::Time(start_scan_time);
success_flag = get_scan_data();
//Send data
publish_scan(data_to_meters(laser_data,SCALE),
start_scan_time, frame_id);
countSeq++;
};
ROS_INFO("Shotdown and close serial: %s.", serialNumber_.c_str());
Cht10driver_.UART0_Close(fd);
// Use a timer to schedule periodic message publishing.
timer_ = node_->create_wall_timer(300ms, publish_message);
}
其中get_scan_data()函數，就是測試代碼內容，得到數據後將數據發送到publish_scan()函數中，就可以發佈傳感器數據了。
編寫可執行程序節點，創建名爲cht10_node_ros.cpp，內容爲：
#include <cht10_node/cht10_node.h>
int main(int argc, char * argv[])
{
// Initialize any global resources needed by the middleware and the client library.
// You must call this before using any other part of the ROS system.
// This should be called once per process.
rclcpp::init(argc, argv);
// Create a node.
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("cht10_node");
cht10_serial_func::Cht10Func cht(node);
// spin will block until work comes in, execute work as it becomes available, and keep blocking.
// It will only be interrupted by Ctrl-C.
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}測試
代碼完成後，按照格式進行編寫CMakeList、package文件，運行catkin_make就可以得到名爲cht10_node.so動態庫，運行驅動節點，命令如下：
ros2 run cht10_node cht10_node_ros 使用ros2 topic echo /range就可以得到傳感器的ROS2消息內容，包括傳感器距離。
目前ROS2功能還未完全開發完畢，一些圖形化還需藉助ROS1來完成。