工作记录——1

其他 2020-04-30 16:38:58 阅读次数: 0

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;
}
罗宾酱
原创文章 15 获赞 8 访问量 924
关注 私信

猜你喜欢

转载自blog.csdn.net/weixin_39652282/article/details/103992120
今日推荐
开源日报 | Chrome内置Gemini的意义不在于Gemini；中国AI追随之路的五大误区；ECharts创始人“下海”养鱼；谷歌I/O开发者大会什么都有，只是没有惊喜
微软回应中国区AI团队“打包赴美”传闻
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
《2024 年一季度互联网投融资运行情况》研究报告
报告：Django 仍然是 74% 开发者的首选
周排行
返回指定时间格式
fopen函数中的mode参数
Java 单例模式探讨
Flex remoteobject工作原理探讨
寻找mplayer的便捷安装方法
30天了解30种技术系列---(26)MySQL自动化运维工具Inception
关于Jboss/Tomcat/Jetty的JNDI定义123
程序减肥，strip，eu-strip 及其符号表
AsyncTask、View.post(Runnable)、ViewTreeObserver三种方式总结frame animation自动启动
Json和Bean的互相转换
每日归档
更多
2024-05-15(24)
2024-05-14(0)
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022