转载自feiliu的github
原址：https://github.com/drfeiliu/pioneer3_control_ros/blob/master/p3_sonar.cpp
大概流程是订阅话题，然后定义回调函数，回调函数输出数据
/***********************************************
Data: 2014-04-01
Author: Fei Liu
File: p3_sonar.cpp
Function: Read (subscribe) sonar data and display it on screen
***********************************************/
/* sensor_msgs/PointCloud
# This message holds a collection of 3d points, plus optional additional
# information about each point.
# Time of sensor data acquisition, coordinate frame ID.
Header header
# Array of 3d points. Each Point32 should be interpreted as a 3d point
# in the frame given in the header.
geometry_msgs/Point32[] points
# Each channel should have the same number of elements as points array,
# and the data in each channel should correspond 1:1 with each point.
# Channel names in common practice are listed in ChannelFloat32.msg.
ChannelFloat32[] channels
*/
/* example of output(using rostopic)
http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud.html
channels: []
---
header:
seq: 143
stamp:
secs: 1396372374
nsecs: 16548440
frame_id: sonar_frame
points:
-1
x: 0.0689999982715 //coordination X
y: 1.19500005245 //coordination Y
z: 0.0
-2
x: 1.27423167229
y: 1.50171017647
z: 0.0
-3
x: 1.08330738544
y: 0.617999970913
z: 0.0
-4
x: 1.09762811661
y: 0.191271170974
z: 0.0
-5
x: 1.11043059826
y: -0.193528607488
z: 0.0
-6
x: 1.12487661839
y: -0.64200001955
z: 0.0
-7
x: 1.00040411949
y: -1.17537534237
z: 0.0
-8
x: 0.0689999982715
y: -2.32699990273
z: 0.0
-9
x: -0.157000005245
y: -2.35500001907
z: 0.0
-10
x: -0.578387975693
y: -0.566369950771
z: 0.0
-11
x: -0.587740302086
y: -0.280499994755
z: 0.0
-12
x: -2.06704616547
y: -0.346512645483
z: 0.0
-13
x: -2.0700006485
y: 0.347033590078
z: 0.0
-14
x: -1.01555681229
y: 0.527499973774
z: 0.0
-15
x: -1.01355516911
y: 1.08498203754
z: 0.0
-16
x: -0.157000005245
y: 1.18700003624
z: 0.0
*/
/* reference
http://stanford-ros-pkg.googlecode.com/svn-history/r146/trunk/recyclerbot/src/object_detector_node.cpp
*/
/*******about the rays and distance*******/
// AT DX front sonar beams
// 3 4 +/-10 degree
// 2 ^ 5 +/-30 degree
// 1 | 6 +/-50 degree
// ___0___center____7____ +/-90 degree
// | | |
// | | |
// | | |
// | P3_ROBOT |
// | | |
// | | |
// | | |
// | | |
// | | |
// |_________|__________|
// AT distance offset(mm):
//0 1 2 3 4 5 6 7
//160 220 240 240 240 240 220 160
/*******about the rays and distance*******/
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h> //for sonar data
#include <math.h>
#include <iostream>
using namespace std;
//in this case, we just use 8 sonars that are in front of the robot
#define SONAR_NUM 8
int offset[SONAR_NUM] ={160, 220, 240, 240, 240, 240, 220, 160};
//sensor_msgs::PointCloud sonarData;
int seq = 0;
int distToObstace[SONAR_NUM];
void get_sonarData_Callback(const sensor_msgs::PointCloud::ConstPtr &sonarScanedData)
{
float tmpX = 0.0, tmpY=0.0;
seq = sonarScanedData->header.seq;
printf("seq of sonar beam and distance measured-->\n");
printf("Frame[%d] : \n", seq);
for (int i=0; i<SONAR_NUM; i++)
{
printf("%d\t",i);
}
printf("\n");
for (int i=0; i<SONAR_NUM; i++)
{
tmpX= sonarScanedData->points[i].x; //coordinate x
tmpY= sonarScanedData->points[i].y; //coordinate y
distToObstace[i] = int(sqrt(tmpX*tmpX+tmpY*tmpY)*1000-offset[i]);
printf("%d\t",distToObstace[i]);
}
printf("\n\n");
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "p3_sonar");
ros::NodeHandle n;
//plsease modify the topic name "/RosAria/sonar" according to the robot connected
ros::Subscriber get_sonar_data = n.subscribe<sensor_msgs::PointCloud>("/RosAria/sonar", 100, get_sonarData_Callback);
printf("\n********** Sonar Readings: **********\n");
while (ros::ok())
{
ros::Duration(0.2).sleep();
/*ROS消息回调处理函数。它俩通常会出现在ROS的主循环中，程序需要不断调用ros::spin() 或 ros::spinOnce()，两者区别在于前者调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而后者在调用后还可以继续执行之后的程序*/
// https://www.cnblogs.com/liu-fa/p/5925381.html
ros::spinOnce();
}
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(sonarvalue)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
sensor_msgs
std_msgs)
catkin_package(
INCLUDE_DIRS include
LIBRARIES sonarvalue
CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs
DEPENDS system_lib
)
include_directories( ${catkin_INCLUDE_DIRS})
add_executable(sonarvalue src/sonar.cpp)
target_link_libraries(sonarvalue ${catkin_LIBRARIES})
robot@ubuntu:~/sonar_pioneer_ws$ rostopic type /RosAria/sonar
sensor_msgs/PointCloud
声呐避障，读取声呐数据，对其排序，然后发布速度消息
#include <stdio.h>
#include <math.h>
#include </usr/local/Aria/include/Aria.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include "geometry_msgs/PoseStamped.h"
#include <sensor_msgs/PointCloud.h> //for sonar data
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h> // can optionally publish sonar as new type pointcloud2
#include <nav_msgs/Odometry.h>
#include "rosaria/BumperState.h"
#include "tf/tf.h"
#include "tf/transform_listener.h" //for tf::getPrefixParam
#include <tf/transform_broadcaster.h>
#include "tf/transform_datatypes.h"
#include <dynamic_reconfigure/server.h>
#include <rosaria/RosAriaConfig.h>
#include "std_msgs/Float64.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Int8.h"
#include "std_msgs/Bool.h"
#include "std_srvs/Empty.h"
#include <sstream>
ros::Publisher robot;
geometry_msgs::Twist cmdvel;
void bubblesort(double *array, int length)
{
int i, j;
for (i = 0; i < length -1; ++i)
{
for (j = 0; j < length - i - 1; ++j)
{
if (array[j] > array[j + 1])
{
double tmp = array[j];
array[j] = array[j + 1];
array[j + 1] = tmp;
}
}
}
}
void readSonar(const sensor_msgs::PointCloud::ConstPtr& msg, sensor_msgs::PointCloud *pointCloud)
{
int i, new_position;
double data[8];
double old_data[8];
//一份数据保存两份，old备份，data排序
for(i = 0; i<8 ; i++)
{
data[i]=msg->points[i].x;
ROS_INFO("I heard sonar: [%lf]", data[i]);
old_data[i]=data[i];
}
for(int i=0 ; i<8; i++){printf("the old data are: [%lf]\n",old_data[i]);}
bubblesort(data,8);
for(int i=0 ; i<8; i++){printf("__the new data are__: [%lf]\n",data[i]);}
//找到距离最大的声呐位置
for(int i=0 ; i<8; i++)
{
if(old_data[i]==data[7])
{
new_position=i;
ROS_INFO("the new position is: [%d]\n",new_position);
}
}
//外层的if-else，如果距离小于2.5则应该stop
if((data[7]>2.5))
{
//障碍物在前面
if((new_position==3)or(new_position==4))
{
cmdvel.linear.x=0.1;
cmdvel.angular.z=0.0;
printf("straight\n");
}
//障碍物在左侧
if((new_position==1)or(new_position==2)or(new_position==3))
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=0.1;
printf("left\n");
}
else if((new_position==4)or(new_position==5)or(new_position==6))
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=-0.1;
printf("right\n");
}
}
else
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=0.0;
printf("stop\n");
}
robot.publish(cmdvel);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "sonar_obstacles_avoidance");
ros::NodeHandle n;
ros::NodeHandle nh;
//sensor_msgs::Joy joyData;
sensor_msgs::PointCloud pointCloud;
//ros::Subscriber realJoystick = n.subscribe<sensor_msgs::Joy>("joy", 1000, boost::bind(readJoy, _1, &joyData));
ros::Subscriber sonar = n.subscribe<sensor_msgs::PointCloud>("/RosAria/sonar", 1000, boost::bind(readSonar, _1, &pointCloud));
//robot = n.advertise<sensor_msgs::Joy>("robot", 1000);
robot = nh.advertise<geometry_msgs::Twist>("/RosAria/cmd_vel", 1000);
ros::Rate loop_rate(100);
while (ros::ok())
{
loop_rate.sleep();
ros::spinOnce();
}
return 0;
}
/*if(msg->points[i].x < 0.5){
cmdvel.linear.x=0.0;
cmdvel.linear.y=0.0;
cmdvel.linear.z=0.0;
cmdvel.angular.x=0.0;
cmdvel.angular.y=0.0;
cmdvel.angular.z=0.0;
}
*/
/*
if((i==3)or(i==4))
{
cmdvel.linear.x=0.1;
cmdvel.angular.z=0.0;
printf("straight\n");
}
else if((i==1)or(i==2))
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=0.1;
printf("left\n");
}
else if((i==5)or(i==6))
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=-0.1;
printf("right\n");
}
else if((i==11)or(i==12))
{
cmdvel.linear.x=-0.1;
cmdvel.angular.z=0.0;
printf("backstraight\n");
}
else if((i==9)or(i==10))
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=-0.1;
printf("right\n");
}
else if((i==13)or(i==14))
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=-0.1;
printf("left\n");
}
else
{
cmdvel.linear.x=0.0;
cmdvel.angular.z=0.0;
printf("stop\n");
}
*/