Time:2020.01.15
本程序是example9的初始程序,包括两个话题发布节点,每个节点都有自己的msg,以及一个话题接收节点。
话题发布节点采用数组的方式,将一帧中的障碍物信息整理到数组中,发布到话题,订阅
节点订阅话题,处理信息,首先是信息放到一个二维数组中,然后距离判断。
talker1.cpp
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <package/talker1.h> //自定义头文件
//#include <vector> //自定义头文件
using namespace std;
class Talker{
public:
Talker(ros::NodeHandle& _nh);
~Talker(){};
void registerPub(); //定义发布者
void pub();
private:
ros::Publisher publisher;//set the publisher as a member
ros::NodeHandle nh;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker1");
ros::NodeHandle n;
Talker talker1(n);
talker1.registerPub();
talker1.pub();
return 0;
}
Talker::Talker(ros::NodeHandle& _nh)
{
nh=_nh;
}
void Talker::registerPub()
{
publisher = nh.advertise<package::talker1>("message1", 1000);
}
void Talker::pub(){
while (ros::ok()){
ros::Rate loop_rate(100);//以10赫兹频率发布消息
package::talker1 talker1msg;// 类型 example6package::talker1msg
int n=4;//检测到的障碍物数量,应是动态可变的, 需要将其实时传递给接收节点。
int m=6*n;//数组长度,6应是固定值,6-1是障碍物特征参数类别数量 对应类别采用数字表示 0是卡车 1是汽车 2是非机动车 3是人
std::vector<double> carray={1.0,1.0,1.0,1.0,1.0,0.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,2.0,1.0,1.0,1.0,1.0,1.0,3.0}; talker1msg.header.stamp=ros::Time::now();
//talker1msg.length=30;
//talker1msg.width=30;
//talker1msg.s=30;
//talker1msg.v=30;
//talker1msg.q=30;
//talker1msg.type="car";
talker1msg.array.data=carray;
talker1msg.s=n;
publisher.publish(talker1msg);
ros::spinOnce();//此处写循环
loop_rate.sleep();
}
}
talker2.cpp
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <package/talker2.h> //自定义头文件
#include <vector>
using namespace std;
class Talker{
public:
Talker(ros::NodeHandle& _nh);
~Talker(){};
void registerPub(); //定义发布者
void pub();
private:
ros::Publisher publisher;//set the publisher as a member
ros::NodeHandle nh;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker2");
ros::NodeHandle n;
Talker talker2(n);
talker2.registerPub();
talker2.pub();
return 0;
}
Talker::Talker(ros::NodeHandle& _nh)
{
nh=_nh;
}
void Talker::registerPub()
{
publisher = nh.advertise<package::talker2>("message2", 1000);
}
void Talker::pub(){
while (ros::ok()){
ros::Rate loop_rate(100);//以10赫兹频率发布消息
package::talker2 talker2msg;
talker2msg.header.stamp=ros::Time::now();
int n=5;//检测到的障碍物数量,应是动态可变的
int m=3*n;//数组长度,3是检测到的每个障碍无的特征数据
vector<double> carray={2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0};
talker2msg.header.stamp=ros::Time::now();
talker2msg.array.data=carray;
talker2msg.s=n;
///talker2msg.s=33;
///talker2msg.v=34;
///talker2msg.q=32;
publisher.publish(talker2msg);
ros::spinOnce();//此处写循环
loop_rate.sleep();
}
}
listener.cpp
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <package/talker1.h> //自定义头文件
#include <package/talker2.h> //自定义头文件
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>//相近对齐
#include <message_filters/time_synchronizer.h>//区别
#include <message_filters/synchronizer.h>//区别
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <sstream>
using namespace std;
using namespace message_filters;
typedef sync_policies::ApproximateTime<package::talker1,package::talker2> MySyncPolicy;
class Listener{
public:
Listener(ros::NodeHandle _nh);//在构造函数中传入节点句柄,并调用 定义订阅节点函数
~Listener(){};
void callback(const package::talker1ConstPtr& msg1,const package::talker2ConstPtr& msg2);
//double result();//定义结果输出函数
private:
ros::NodeHandle nh;
//定义消息同步机制和成员变量
message_filters::Subscriber<package::talker1>* sub1 ; // topic1 输入
message_filters::Subscriber<package::talker2>* sub2; // topic2 输入
message_filters::Synchronizer<MySyncPolicy>* sync;
int length,width,s,v,q,length1,width1,s1,v1,q1,s2,v2,q2;
string type,type1,type2;
double Result;
};
Listener::Listener(ros::NodeHandle _nh)
{
//类构造函数中开辟空间new
nh=_nh;
sub1 = new message_filters::Subscriber<package::talker1>(nh, "message1", 1000);
sub2 = new message_filters::Subscriber<package::talker2>(nh, "message2", 1000);
sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10), *sub1, *sub2);
sync->registerCallback(boost::bind(&Listener::callback,this, _1, _2));
}
void Listener::callback(const package::talker1ConstPtr& msg1,const package::talker2ConstPtr& msg2)
{
//类成员函数回调处理
//输出相机检测到几个障碍物:
int n1=msg1->s;
cout<<"相机检测到的障碍物数量是:"<<n1<<endl;
//输出激光雷达检测到几个障碍物:
int n2=msg2->s;
cout<<"激光雷达检测到的障碍物数量是:"<<n2<<endl;
//相机的检测到的障碍物的位置、角度、速度依次输出(放到一个矩阵中):
//定义存放相机检测到的障碍物信息的矩阵(n1*6):
int m=0,n=0;
vector<vector<double>> msg1vector(n1,vector<double>(6));
for(int i=0;i<n1;i++){
for(int j=0;j<6;j++){
msg1vector[i][j]=msg1->array.data[m];//把相机检测到的每个障碍物的特征信息放到矩阵中:
//cout<<"msg1vector"<<i<<"."<<j<<" "<<msg1vector[i][j]<<endl;
m=m+1;
}
}//此时相机检测到的障碍物信息已经被存放在二位数组msg1vector中
vector<vector<double>> msg2vector(n2,vector<double>(3));//定义存放激光雷达检测到的障碍物信息的矩阵(n2*3):
for(int i=0;i<n2;i++){
for(int j=0;j<3;j++){
msg2vector[i][j]=msg2->array.data[n];//把激光雷达检测到的每个障碍物的特征信息放到矩阵中:
//cout<<"msg2vector"<<i<<"."<<j<<" "<<msg2vector[i][j]<<endl;
n=n+1;
}
}//此时激光雷达检测到的障碍物信息已经被存放在二位数组msg2vector中。
//欧式距离实现:
//msg1中的第一行与msg2中的各行依次比较;
//定义一个矩阵存放欧式距离计算结果、msg1中有4个障碍物,msg2中有5个障碍物,那么矩阵的维数是4*5,即n1*n2(矩阵1的行数×矩阵2的行数);
vector<vector<double>> distanceresultV(n1,vector<double>(n2));
for(int i=0;i<n1;i++){
for(int j=0;j<n2;j++){
double d=0.0; //需要注意d的位置
for(int k=0;k<3;k++){//数组a与数组b的列是一样的,因此k代表他们的列数。
cout<<"msg1vector"<<i<<"."<<j<<" "<<msg1vector[i][k]<<endl;
cout<<"msg2vector"<<i<<"."<<j<<" "<<msg2vector[j][k]<<endl;
d+=(msg1vector[i][k]-msg2vector[j][k])*(msg1vector[i][k]-msg2vector[j][k]); //注意此处是msg1vector-msg2vector
Result=sqrt(d);
}
distanceresultV[i][j]=Result;
cout<<"distanceresultV"<<i<<"."<<j<<" "<<distanceresultV[i][j]<<endl;
}
}
//标准欧式距离实现:
//注意:判断分母是否为零
//msg1中的第一行与msg2中的各行依次比较;
//定义一个矩阵存放欧式距离计算结果、msg1中有4个障碍物,msg2中有5个障碍物,那么矩阵的维数是4*5,即n1*n2(矩阵1的行数×矩阵2的行数);
vector<vector<double>> distanceresultV(n1,vector<double>(n2));
for(int i=0;i<n1;i++){
for(int j=0;j<n2;j++){
double d=0.0;
for(int k=0;k<3;k++){//数组a与数组b的列是一样的,因此k代表他们的列数。
/*vector<double> u(3); 应该也可以用vector实现,没有尝试*/
double u[3],s[3];
u[k]=(msg1vector[i][k]+msg2vector[j][k])/2;//计算平均值
s[k]=(msg1vector[i][k]-u[k])*(msg1vector[i][k]-u[k]);//计算分母
//判断分母是否为零:
if(s[k])
{
d+=(msg1vector[i][k]-msg2vector[j][k])*(msg1vector[i][k]-msg2vector[j][k])/s[k]; //计算标准欧式距离的关键步骤
Result=sqrt(d);
}
}
distanceresultV[i][j]=Result;
cout<<"distanceresultV"<<i<<"."<<j<<" "<<distanceresultV[i][j]<<endl;
}
}
//匈牙利匹配算法
}
/*double Listener::result()//定义结果输出函数
{
cout<<"欧式距离为:"<<endl;
return Result;
}*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle nh;
Listener listener(nh);
//listener.result();
ros::spin();
return 0;
}