ROS入门 Ubuntu接入usb 读取超声测距

Posted 2020-10-20 大G霸

我们选用超声模块接入Arduino，

接线 Trig口接入2口，Echo接入3口。

const int TrigPin = 2;
const int EchoPin = 3; 
int cm;
unsigned long time_ms = 0;
void setup() {
  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
}

void loop() {

digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin 
delayMicroseconds(1); 
digitalWrite(TrigPin, HIGH); 
delayMicroseconds(1); 
digitalWrite(TrigPin, LOW); 

cm = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm 
cm = (int(cm * 100.0)) / 100.0; //保留两位小数 
time_ms = millis();
//Serial.print(time_ms); 
//Serial.println(" "); 
Serial.print(cm); 
Serial.println("");
delay(33); 
}

View Code

USB打印出来以int显示。

#include <ros/ros.h>
#include "geometry_msgs/Twist.h"

#include <boost/asio.hpp>         // 包含boost库函数
#include <boost/bind.hpp>
#include <sstream>
#include <string>

using namespace std;
using namespace boost::asio;
unsigned char buf[24];

int main(int argc, char** argv){
  ros::init(argc, argv, "echo");
  ros::NodeHandle n;

  ros::Publisher echo_msg = n.advertise<geometry_msgs::Twist>("echo/stop", 1000);

  io_service io;
  serial_port sp(io, "/dev/ttyACM0");         // 定义传输的串口
  sp.set_option(  serial_port::baud_rate(9600));                                  // 波特率
  sp.set_option(  serial_port::flow_control( serial_port::flow_control::none ) ); // 流量控制
  sp.set_option(  serial_port::parity( serial_port::parity::none ) );             // 奇偶校验
  sp.set_option(  serial_port::stop_bits(  serial_port::stop_bits::one ) );        // 停止位
  sp.set_option(  serial_port::character_size( 8 ) );                             // 数据位



  ros::Rate loop_rate(300);
  int num;
  int read_num = 0 ;
  int flag = 0;
  while(ros::ok())
  {
    int num = 0;
    char buf[1];
    boost::system::error_code err;
    read(sp, buffer(buf));
    if (err)
    {
        ROS_INFO("Serial port read_some Error!");
        return -1;
    }
    if(buf[0] == ‘\n‘)
    {
      printf("%d \n", read_num/10);
      read_num = 0;
    }else {
      std::stringstream ss;
      ss << buf[0];
      ss >> num;
      read_num = read_num * 10 + num;
    }

    geometry_msgs::Twist velo;
    if(read_num/10 <= 30){
      //publish a msg
    }else{

    }

    loop_rate.sleep();
  }
  sp.close();
  return 0;
}

最后结合之前发过的ubuntu读取USB，读取距离并将数据传入。

USB接入完成。