ROS中启动超声波雷达节点「建议收藏」

Jetbrains全系列IDE使用 1年只要46元 售后保障 童叟无欺

超声波雷达为深圳导向机电的，型号为KS136,KS136 使用 I 2 C 接口与主机通信，自动响应主机的 I 2 C 控制指令。指令为８位数据，指令发
送流程如下，首先向超声波接受器写入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}
)