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