超聲波雷達(dá)為深圳導(dǎo)向機(jī)電的，型號為KS136,KS136 使用 I 2 C 接口與主機(jī)通信，自動(dòng)響應(yīng)主機(jī)的 I 2 C 控制指令。指令為８位數(shù)據(jù)，指令發(fā)
送流程如下，首先向超聲波接受器寫入I2C地址為0xc8，寄存器地址0x02，超聲波探頭號地址0x10，

程序?yàn)?

Robot_Serial.write(writebuff, sizeof(writebuff))

再向buffer中讀數(shù)據(jù)

Robot_Serial.read(Reciver_data.buffer, sizeof(Reciver_data.buffer))

流程圖如下所示：

超聲波與主控之間通過串口通信代碼如下：

ros::Publisher chatter_pub = n.advertise<std_msgs::Float32MultiArray>("/ultrasonic_distance", 1000);if (bIsRviz == true){for (loop = 0;loop < 11 ;loop++){ssloop.clear();ssloop.str("");ssloop << loop+1;Str_sonar_pub.clear();Str_sonar_pub.append("/ultrasonic");Str_sonar_pub.append(ssloop.str());mapSonar_pub[loop]=n.advertise<sensor_msgs::Range>(Str_sonar_pub, 1000);}}ros::Subscriber sonar_sub = n.subscribe("/sonar_list", 100, sonar_callback);start_time = ros::Time::now();while (ros::ok()){//--------------------------------------trick by callback /sonar_listif (sonar_callbackflag == true){count = 0;sonar_callbackflag = false;}else{if (sonar_data[sonar_order[count]].bIsdetect == true){sonar_start_time = ros::Time::now();writebuff[0] = 0xe8;writebuff[1] = 0x02;if (sonar_type == KS136){writebuff[2] = sonar_mod+8*sonar_order[count];}else if (sonar_type == KS106){//zzzzzzzzzzif (sonar_order[count] == 2-1//||sonar_order[count] == 6-1//||sonar_order[count] == 10-1){writebuff[2] = sonar_mod + 8*4;}else if (//sonar_order[count] == 2-1sonar_order[count] == 6-1//||sonar_order[count] == 10-1){writebuff[2] = sonar_mod + 8*5;}else if (//sonar_order[count] == 2-1//||sonar_order[count] == 6-1sonar_order[count] == 10-1){writebuff[2] = sonar_mod + 8*6;}}//-----------------------------------------------------to get Exception iotry{Robot_Serial.write(writebuff, sizeof(writebuff));Robot_Serial.read(Reciver_data.buffer,sizeof(Reciver_data.buffer));}catch(serial::IOException& e){ROS_INFO("[Serial] something wrong %s ",e.what());ros::Duration(5).sleep();return 0;}//-----------------------------------------------------to get Exception io//--------------------------------------------------------decode serial datadist = Reciver_data.buffer[0]*0xff+Reciver_data.buffer[1];//--------------------------------------------------------decode serial data//--------------------------------------------------------turn us to mmif (sonar_mod == 0x12 || sonar_mod == 0x17){dist = (float)340*1000*dist/1000000/2;}//--------------------------------------------------------turn us to mm//-------------------------------------------------------turn mm to cm for jiangzhiyongsonar_data[sonar_order[count]].dist = dist /10;//-------------------------------------------------------turn mm to cm for jiangzhiyong//-----------------------------------calc time one sonar sonar_end_time = ros::Time::now();sonar_pertime = (sonar_end_time - sonar_start_time).toNSec(); if (sonar_delta == 0){//do nothing}else{if (sonar_pertime > sonar_delta*1000000 ){//do nothing}else{ros::Duration(0,(sonar_delta*1000000 - sonar_pertime)).sleep();}}//-----------------------------------calc time one sonar}else{//----------------------------------------------------default value to send sonar_data[sonar_order[count]].dist = 10000 /10;}count++;if (count >= 11){//------------------------------------------------------turn zerocount = 0;//------------------------------------------------------turn zero//------------------------------------------------------send msgfollow_msg.data.clear();for (loop = 0;loop < 11 ;loop++){if (sonar_data[loop].bIsdetect == true){follow_msg.data.push_back(sonar_data[loop].dist);}}chatter_pub.publish(follow_msg);//-----------------------------------------------------send msg//-----------------------------------------------------send msg rviz

CmakeLists.txt文件如下

cmake_minimum_required(VERSION 2.8.3) project(sonar_pub)find_package(catkin REQUIRED)find_package(catkin REQUIRED COMPONENTSroscpp serial )catkin_package()include_directories(include ${catkin_INCLUDE_DIRS})add_executable(sonar_pub_pubsrc/sonar_pub_pub.cpp ) add_dependencies(sonar_pub_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(sonar_pub_pub${catkin_LIBRARIES} )

?

總結(jié)

以上是生活随笔為你收集整理的ROS中启动超声波雷达节点的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。