超聲波雷達爲深圳導向機電的,型號爲KS136,KS136 使用 I 2 C 接口與主機通信,自動響應主機的 I 2 C 控制指令。指令爲8位數據,指令發
送流程如下,首先向超聲波接受器寫入I2C地址爲0xc8,寄存器地址0x02,超聲波探頭號地址0x10,
程序爲:
Robot_Serial.write(writebuff, sizeof(writebuff))
再向buffer中讀數據
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_list
if (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)
{
//zzzzzzzzzz
if (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-1
sonar_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-1
sonar_order[count] == 10-1
)
{
writebuff[2] = sonar_mod + 8*6;
}
}
//-----------------------------------------------------to get Exception io
try
{
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 data
dist = Reciver_data.buffer[0]*0xff+Reciver_data.buffer[1];
//--------------------------------------------------------decode serial data
//--------------------------------------------------------turn us to mm
if (sonar_mod == 0x12 || sonar_mod == 0x17)
{
dist = (float)340*1000*dist/1000000/2;
}
//--------------------------------------------------------turn us to mm
//-------------------------------------------------------turn mm to cm for jiangzhiyong
sonar_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 zero
count = 0;
//------------------------------------------------------turn zero
//------------------------------------------------------send msg
follow_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 COMPONENTS
roscpp serial
)
catkin_package(
)
include_directories(
include ${catkin_INCLUDE_DIRS}
)
add_executable(sonar_pub_pub
src/sonar_pub_pub.cpp
)
add_dependencies(sonar_pub_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(sonar_pub_pub
${catkin_LIBRARIES}
)