标签：nh include 记录 int talker1msg 工作 Talker ros
Time : 2020.01.21
发现之前的标准欧式距离的公式理解错了，导致每次运行算出来的结果都是4，所以修改了程序，并记录在这。
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 <cstdio>
#include <stdlib.h>
#include <string>
#include <sstream>
#include <algorithm>
using namespace std;
using namespace message_filters;
typedef sync_policies::ApproximateTime<package::talker1,package::talker2> MySyncPolicy;
//匈牙利算法用到的
#define MAXN 990 //MAXN表示X集合和Y集合顶点个数的最大值
class Listener{
public:
Listener(ros::NodeHandle _nh);//在构造函数中传入节点句柄，并调用 定义订阅节点函数
~Listener(){};
void callback(const package::talker1ConstPtr& msg1,const package::talker2ConstPtr& msg2);
int path(int u);
int maxMatch();
private:
ros::NodeHandle nh;
int length,width,s,v,q,length1,width1,s1,v1,q1,s2,v2,q2,n1,n2;
string type,type1,type2;
double Result;
int g[MAXN][MAXN]; //邻接矩阵，g[i][j]=1表示有连接
int cx[MAXN],cy[MAXN]; //cx[i],表示最终求得的最大匹配中，与x集合中元素Xi匹配的集合Y中顶点的索引//cy[i],表示最终求得的最大匹配中，与y集合中元素Yi匹配的集合X中顶点的索引
int mk[MAXN]; //DFS算法中记录顶点访问状态的数据mk[i]=0表示未访问过，为1表示访问过，标记的永远是二分图右边的顶点。
//定义消息同步机制和成员变量
message_filters::Subscriber<package::talker1>* sub1 ; // topic1 输入
message_filters::Subscriber<package::talker2>* sub2; // topic2 输入
message_filters::Synchronizer<MySyncPolicy>* sync;
};
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)
{
//类成员函数回调处理
//输出相机检测到几个障碍物：
n1=msg1->s;
//输出激光雷达检测到几个障碍物：
n2=msg2->s;
cout<<n2<<endl;
//相机的检测到的障碍物的位置、角度、速度等依次输出（放到一个矩阵中(n1*6)）：
int m=0,n=0;
double array[n2];
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]<<endl;
m=m+1;
}
}
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]<<endl;
n=n+1;
}
}
/*
//欧式距离
//定义一个矩阵存放欧式距离计算结果、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代表他们的列数。
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中有4个障碍物，msg2中有5个障碍物，那么矩阵的维数是4*5，即n1*n2（矩阵1的行数×矩阵2的行数）；
//////////////////////////////////////////////////////////////////////////////////////////
double U[3]={0,0,0};
double u1[3]={0,0,0};
double u2[3]={0,0,0};//定义存放均值的参数；u1、u2分别存放msg1vector、msg2vector；U存放最终的数据；
double S[3]={0,0,0};
double s1[3]={0,0,0};
double s2[3]={0,0,0};//定义存放均方根的参数；s1、s2分别存放msg1vector、msg2vector每列的数据；S[3]存放最终的数据；
for(int i=0;i<3;i++){
for(int j=0;j<n1;j++){
//u1[i]+=msg1vector[j][i];//u1[0]存放的是第一组的 第一列，u1[1]存放的是第二列，u1[2]存放的是第三列；
}
cout<<"u1"<<i<<" "<<u1[i]<<endl;
}
for(int i=0;i<3;i++){
for(int j=0;j<n2;j++){
u2[i]+=msg2vector[j][i];//u2[0]存放的是第二组的 第一列，u2[1]存放的是第二列，u2[2]存放的是第三列；
//cout<<"u2"<<i<<" "<<u2[i]<<endl;
}
}
//求数组中每一列的均值
for(int i=0;i<3;i++){
U[i]=(u1[i]+u2[i])/(n1+n2);//U[0]存放的是第一列的均值，U[1]存放的是第二列的均值，U[2]存放的是第三列的均值；
//cout<<"U"<<i<<" "<<U[i]<<endl;
}
//MSG1VECTOR第一列数据、第二列数据、第三列数据
for(int i=0;i<3;i++){
for(int j=0;j<n1;j++){
s1[i]+=(msg1vector[j][i]-U[i])*(msg1vector[j][i]-U[i]);//s1[0]存放的是第一列，s1[1]存放的是第二列，s1[2]存放的是第三列；
}
}
//MSG2VECTOR第一列数据、第二列数据、第三列数据
for(int i=0;i<3;i++){
for(int j=0;j<n2;j++){
s2[i]+=(msg2vector[j][i]-U[i])*(msg2vector[j][i]-U[i]);//s1[0]存放的是第一列，s1[1]存放的是第二列，s1[2]存放的是第三列；
}
}
//求数组中每一列的均方根
for(int i=0;i<3;i++){
S[i]=(s1[i]+s2[i])/(n1+n2);//U[0]存放的是第一列的均值，U[1]存放的是第二列的均值，U[2]存放的是第三列的均值；
//cout<<"S"<<i<<" "<<S[i]<<endl;
}
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代表他们的列数。
//判断分母是否为零：
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; //输出第一组数据和第二组数据之间的距离
}//distanceresultV[i][j]矩阵就是接下来要处理的数据
}
/* 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代表他们的列数。
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;
}
}//distanceresultV[i][j]矩阵就是接下来要处理的数据
*/
// 找到每行的最小值及次小值，并将其置为1，其余值置为0
for(int i=0;i<n1;i++){
for(int j=0;j<n2;j++){
array[j]=distanceresultV[i][j];//先取出来距离矩阵的第一行
}
double m1,m2;//存储两个最小值
m1=9999;
m2=9999;
for(int k=0;k<n2;k++){
if(array[k]<m1){
m2=m1;
m1=array[k];
}
else if(array[k]<m2){
m2=array[k];
}
}
cout<<m1<<" / "<<m2<<endl;
for(int l=0;l<n2;l++){
if(array[l]==m1){
//array[l]=1;
g[i][l]=1;
}
else if(array[l]==m2){
//array[l]=1;
g[i][l]=1;
}
else{
g[i][l]=0;
}
}
}
for(int m=0;m<n1;m++){
for(int n=0;n<n2;n++){
cout<<"g["<<m<<"]"<<"["<<n<<"]"<<g[m][n]<<endl;
}
}
//在回调函数中调用匈牙利匹配算法
int num= Listener::maxMatch();//调用匈牙利算法最核心的步骤//输出了有多少组匹配的障碍物
cout<<"num="<<num<<endl;
//比较n1、n2大小;
int t=n1>n2?n1:n2;
cout<<t<<endl;
//
for(int i=0;i<t;++i){
cout<<"cx["<<i+1<<"] --> "<<cx[i]+1<<endl;
int q=cx[i];
//cout<<"cx[i]+1: "<<cx[i]+1<<endl;
//cout<<"q: "<<cx[i]<<endl;
//相机的第p+1个目标与激光雷达的第cx[p]+1个对象相对应
cout<<"相机数据1: "<<msg1vector[i][0]<<endl;
cout<<"相机数据2: "<<msg1vector[i][1]<<endl;
cout<<"相机数据3: "<<msg1vector[i][2]<<endl;
cout<<"激光雷达数据1: "<<msg2vector[q][0]<<endl;
cout<<"激光雷达数据2: "<<msg2vector[q][1]<<endl;
cout<<"激光雷达数据3: "<<msg2vector[q][2]<<endl;
//给相机和激光雷达的速度、方向以及距离的测量值分别附一个权重 输出（msg1和msg2均参与计算）；输出障碍物的类别、长、宽（输出msg1相对应的. 就可以了）
}
}
//开始处理
//从集合X中的定顶点u出发，用深度有限的策略寻找增广路
//这种增广路只能是当前的匹配数增加1
int Listener::path(int u){ //传入的是左侧的顶点
for(int v=0;v<n2;++v){ //考虑所有右侧顶点，从第一个开始扫描，依次向下
if(g[u][v] && !mk[v]){ //如果左侧有右侧某顶点相连接，并且这个右侧顶点没有被此时的左侧顶点访问过
mk[v]=1; //标记右侧顶点，访问v
//接下来，判断这个右侧顶点。
//如果v没有匹配，则直接将v匹配给u。或者，如果v已经匹配了，但是从cy[v],也就是从v之前已经匹配的x出发，找到一条增广路，但是这里记住这里v已经记录访问过了
//如果第一个条件成立，则不会递归调用
if(cy[v]==-1 || path(cy[v])){ //path()函数的返回值是1或者0；
//如果满足上述条件，更新cx cy
cx[u]=v; //把Y中v匹配给X中u
cy[v]=u; //把X中u匹配给Y中v
return 1; //找到左侧点的匹配值，那么将返回1
}
}
}
return 0; //如果不存在从u出发的增广路，则返回0
//具体的说：如果左侧点找不到一个匹配点，那么将会返回0，此时path(cy[v])=0，就不会将右侧点与之前匹配的点拆散，而是左侧点重新依次向下寻找右侧链接顶点
}
int Listener::maxMatch(){ //求二分图最大匹配的匈牙利算法；
int res=0;
memset(cx,-1,sizeof(cx)); //从0匹配开始增广，将cx和xy各元素都初始化为-1；
memset(cy,-1,sizeof(cy));
for(int i=0;i<n1;++i){ //开始依次从左侧顶点匹配；
if(cx[i]==-1){ //从X集合中每个没有匹配的点出发开始寻找增广路；
memset(mk,0,sizeof(mk)); //每个左侧顶点匹配之前都要初始化；
res+=Listener::path(i); //输出匹配成功个数，或者if(path(i)) res++
}
}
return res;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle nh;
Listener listener(nh);
//listener.result();
ros::spin();
return 0;
}
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
talker1msg.header.stamp=ros::Time::now();
int n=4;//检测到的障碍物数量,应是动态可变的， 需要将其实时传递给接收节点。
int m=6*n;//数组长度,6应是固定值,6-1是障碍物特征参数类别数量 对应类别采用数字表示 0是卡车 1是汽车 2是非机动车 3是人
std::vector<double> carray={5.3,1.4,2.4,1.0,3.0,0.0,3.4,6.2,2.2,1.0,1.5,1.0,3.2,5.2,3.0,1.3,1.0,2.0,4.5,2.1,5.2,3.2,1.2,3.0};
//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={3.2,1.5,1.7,2.3,2.5,3.3,1.8,1.4,4.4,1.1,3.2,2.1,3.1,1.5,2.6};
talker2msg.array.data=carray;
talker2msg.s=n;
///talker2msg.s=33;
///talker2msg.v=34;
///talker2msg.q=32;
publisher.publish(talker2msg);
ros::spinOnce();//此处写循环
loop_rate.sleep();
}
}
罗宾酱
发布了10 篇原创文章 · 获赞 6 · 访问量 303
私信
关注
标签：nh,include,记录,int,talker1msg,工作,Talker,ros 来源： https://blog.csdn.net/weixin_39652282/article/details/104060964
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。