【ROS總結】點激光掃描儀數據發佈（驅動編寫）

原創 ros小生 2018-09-03 05:42

前言

  在使用ROS的過程中，不可避免的會遇到要自己手動編寫驅動、節點、行爲庫、服務、消息等內容，尤其是需要一些傳感器，而所需傳感器沒有相關ROS驅動時，下面將講述的是在使用的傳感器中，沒有ROS驅動時，我們將如何使用該傳感器。

 

  在接下來的博客中將根據本身經驗來編寫一些ROS相關內容，權當是作爲記憶來分享。


ROS驅動——點激光掃描儀


  下面將講述使用ROS官方未有的傳感器來作爲樣例，我使用的是萊旭光電的CHT-10點激光傳感器作爲 介紹，首先這款傳感器通信協議部分比較簡單，基本上打開串口就可以讀出數據，在將數據進行轉換就可以得到想要的距離，比較方便，免去複雜的部分，對教程也有幫助，畢竟教程主要講述的是ROS驅動部分。


  相關ROS驅動代碼可以在github上找到進行下載：https://github.com/Playfish/cht10_node


  CHT-10是點激光，掃描範圍是0.05-10m，真實的應該是0.15-10m，10m未測試，不過0.15盲區還是有的。

  下圖是CHT-10的通信協議部分介紹：

  可以看到想要的數據在9~12位（四字節），得到的數據需要除以1000才能得到真實的距離（米），ROS消息中基本單位是米。


消息選定

  關於選擇什麼消息作爲該傳感器的ROS消息，這個相對來說不是很重要，不過作爲通用項來說，儘量向通用靠近（即使用ROS官方消息定義，而不是自己定義消息）。目前激光消息的話，有兩個選擇，第一個是sensor_msgs/LaserScan消息類型，不過該消息類型適合有角度的傳感器，即180°或360°的2D雷達或激光，而目前使用的激光是單點類型，和激光筆差不多，只有一個點，那麼可以選擇sensor_msgs/Range消息類型，該消息類型適用於超聲波傳感器、紅外傳感器單點類型，CHT-10就用這個類型。


關於該消息：可以查看sensor_msgs/Range


準備工作

  傳感器選型選定、通信消息選定，那麼接下來還需要做：

  • Frame_id：Frame在ROS中作用至關重要，消息將和TF綁定纔可以讀取數據，在這裏作爲通用可配置，暫定內容爲：laser，用戶可自定義設置（通過ROS Parameters設置）。
  • 串口：避免和其他傳感器串口衝突，因此在這裏預留出一個串口設置參數，用戶可以自定義設置（通過ROS Parameters設置）。
  • 話題：消息內容需要通過話題發佈，並且話題需要唯一，不然容易崩潰，在這裏選擇話題爲“range”

  當然以上部分可以不考慮，這個是作爲通用型需要考慮的，暫時可以先忘記以上部分，下面開啓代碼部分


測試代碼編寫


  在這部分，主要先編寫測試代碼，如果測試代碼無問題，就可以寫到node中，測試代碼和ROS無關係，只是通過串口讀取數據，並且在終端顯示出來，整體思路如下：


  傳感器選擇 -> 測試程序測試（類似串口助手） -> ROS node綁定（將測試程序讀取部分加入到ROS中） -> 將node加入到nodelet中封裝


  下面是測試程序部分，也可以在cht10_node/test中找到，名爲test_cht10.cpp：

#include <string>
#include <cht10_node/seiral_func.hpp>
#include <cstdlib>

#include <iostream>
#include <stdint.h>
#define BUFSIZE 17

int main(int argc, char** argv){
  cht10_seiral_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];
  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
          */
//          std::cout<<"Received data, with :[";
//          for(int j=0;j<BUFSIZE;j++){
//            printf("%c ",(unsigned char) result_buf[j]);
//          }
//          printf("] \n");
          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，該類包含串口讀/寫部分、初始化。

  通過使用catkin_make可以得到一個名爲test_cht10的可執行文件，使用./test_cht10可以運行，將CHT-10插入到電腦上，並且電腦只有一個ttyUSB0，就可以讀取數據，數據將顯示爲sensor data: 傳感器毫米數據， Distance: 傳感器米數據。


ROS驅動編寫


  上部分講述了測試程序編寫，主要作用是通過串口讀取傳感器數據，那麼得到傳感器數據之後，就可以將傳感器數據填充到ROS消息中，然後通過話題形式發佈出去，如下是將傳感器數據填充到ROS消息並封裝成nodelet類的主要部分：


  完整代碼可以查看： cht10_node.cpp

/**
 * @file /Cht10_serial_func/src/Cht10_serial_func.cpp
 *
 * @brief Implementation for dirver with read data from Cht10 nodelet
 *
 * @author Carl
 *
 **/

/*****************************************************************************
 ** Includes
 *****************************************************************************/
#include <string>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <nodelet/nodelet.h>
#include <ecl/threads/thread.hpp>
#include <sensor_msgs/LaserScan.h>  
#include <sensor_msgs/Range.h>  
#include <pluginlib/class_list_macros.h>
#include <cht10_node/seiral_func.hpp>


namespace cht10_seiral_func{

class Cht10Func : public nodelet::Nodelet{
#define BUFSIZE 17
#define SCALE 1000

public:
  Cht10Func() : shutdown_requested_(false),serialNumber_("/dev/USB0"),frame_id("laser"){}

  ~Cht10Func(){
    NODELET_DEBUG_STREAM("Waiting for update thread to finish.");
    shutdown_requested_ = true;
    update_thread_.join();
  }

  virtual void onInit(){

    ros::NodeHandle nh = this->getPrivateNodeHandle();
    std::string name = nh.getUnresolvedNamespace();
    nh.getParam("serialNumber", serialNumber_);
    nh.getParam("baudRate", baudRate_);
    nh.getParam("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_STREAM("Open Serial: "<<serialNumber_.c_str()<<" Error!");
      exit(0);
    }

    int countSeq=0;
    scan_pub = nh.advertise<sensor_msgs::Range>("range",100);
    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_STREAM("Open serial: ["<< serialNumber_.c_str() <<" ] successful, with idex: "<<fd<<".");
    NODELET_INFO_STREAM("Cht10Func initialised. Spinning up update thread ... [" << name << "]");
    update_thread_.start(&Cht10Func::update, *this);
  }

  double data_to_meters(int &data, int scale){
    return (double)data/scale;
  }

  void publish_scan(ros::Publisher *pub,
                    double nodes, ros::Time start,
                    std::string frame_id){

    float final_range;
    sensor_msgs::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::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.header.seq = countSeq;
    range_msg.range = final_range;
    scan_pub.publish(range_msg);

  }

  bool 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    
    }
  }
private:
  void update(){
    rcv_cnt = 0;
    success_flag = 0;
    laser_data = 0;
    ros::Rate spin_rate(50);
    memset(buf, 0, sizeof(buf));
    memset(temp_buf, 0, sizeof(temp_buf));
    memset(result_buf, 0, sizeof(result_buf));
    ROS_INFO_STREAM("Begin receive data from "<<serialNumber_.c_str()<<", with idex:"<<fd<<".");
    fd = open(serialNumber_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );
    Cht10driver_.UART0_Init(fd,baudRate_,0,8,1,'N');
    while (! shutdown_requested_ && ros::ok())
    {
      start_scan_time = ros::Time::now();
      success_flag = get_scan_data();
          
      //Send data
      publish_scan(&scan_pub, data_to_meters(laser_data,SCALE),
                       start_scan_time, frame_id);
      spin_rate.sleep();

      countSeq++;
    }

    ROS_INFO_STREAM("Shotdown and close serial: "<<serialNumber_.c_str()<<".");
    Cht10driver_.UART0_Close(fd);
  }
private:
  int fd, len, rcv_cnt;
  int success_flag;
  char buf[40], temp_buf[BUFSIZE],result_buf[BUFSIZE];
  
  Cht10Driver Cht10driver_;
  ecl::Thread update_thread_;
  bool shutdown_requested_;
  ros::Publisher scan_pub;
  int laser_data;
  char data_buf[4];
  // ROS Parameters
  std::string serialNumber_;
  int baudRate_;
  int countSeq;
  
  std::string frame_id;

  ros::Time start_scan_time;
};

} //namespace Cht10_serial_func
PLUGINLIB_EXPORT_CLASS(cht10_seiral_func::Cht10Func,
nodelet::Nodelet);

  其中get_scan_data()函數，就是測試代碼內容，得到數據後將數據發送到publish_scan()函數中，就可以發佈傳感器數據了。

 

測試

  代碼完成後，按照格式進行編寫CMakeList、package文件，運行catkin_make就可以得到名爲cht10_node.so動態庫，運行launch文件即可加載，命令如下：

roslaunch cht10_node standalone.launch

  如果一切成才，運行rostopic list 可以得到/range話題。

  使用rostopic echo /range就可以得到傳感器的ROS消息內容，包括傳感器距離。

 或者直接運行

roslaunch cht10_node view_range.launch

 就可以看到圖形化形式爲傳感器數據。

發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章