码迷,mamicode.com
首页 > 系统相关 > 详细

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

时间:2018-01-08 10:57:50      阅读:295      评论:0      收藏:0      [点我收藏+]

标签:echo   string   turn   igp   hid   ret   nod   dev   str   

我们选用超声模块接入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接入完成。

 

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

标签:echo   string   turn   igp   hid   ret   nod   dev   str   

原文地址:https://www.cnblogs.com/TIANHUAHUA/p/8241334.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!