一.概述

二.ROS话题传输

①发布者 RobotX.cpp

②订阅者 MapPointMerge.cpp

三.msg文件的定义：

四.代码修改部分

①点的传输数目 RobotX.cpp

②位姿传输

五、效果图

六、任务

七、小喝彩

星期六- 复现第二阶段点与关键帧

一.概述
在师兄给的初始文件上进行修改，ROS机器人添加SLAM功能。

根据师兄要求，首先是对传输融合地图点的数目进行修改，其次是给融合地图添加一个”画关键帧“的功能。

二.ROS话题传输

①发布者 RobotX.cpp：

	void ThreadSendMessage()                              //定义构造函数
	{
	    ros::NodeHandle pubNH;                     //创建节点句柄：pubNH
	    ros::Publisher pub = pubNH.advertise("MapPoint_info", 1);    
	    
	    ros::Rate loop_rate(1.0);                       //设置循环的频率
	    while(ros::ok())                                                                                                 //关于ROS节点以及发布者的核心代码
	    {
				………………        
	   }
	int main(int argc, char** argv){
		  ros::init(argc, argv, ROS_TOPIC_NAME);                                                               //初始化ROS节点
		  ros::start();
		  std::thread PubMessage(ThreadSendMessage);      //构造函数,增加副线程完成工作，同时工作，减少耗时
		  PubMessage.join();          
		
		  return 0;
		
		}

②订阅者 MapPointMerge.cpp：

		int main(int argc,char** argv)
		{
			ros::init(argc,argv,"ORB_SLAM3_MultiRobot");           //和发布者一样，固定步骤，先初始化节点：ORB_SLAM3_MultiRobot
			ros::NodeHandle nodeHandler;                                        //创建节点句柄 nodeHandler
			std::thread ThreadDrawMap(DrawMap);                        //增加副线程，画图
			  //接收
			ros::Subscriber sub = nodeHandler.subscribe("MapPoint_info",1,AddPointToVector);  //对应发布者的节点名，队列长度1，输入回调函数
			ros::Subscriber sub_ = nodeHandler.subscribe("MapPoint_info",1,AddTMatrixToVector);  
			ros::spin();    
			
			ThreadDrawMap.join();
			    return 0;
		}

三.msg文件的定义：

ROS消息由每个ROS包的msg目录中的消息定义文件说明。这些文件会被编译成与编程语言有关的的实现版本，这样你才能在写代码的时候使用这些消息结构。这意味着及时你使用的是一种解释型的编程语言，例如Python。你依然需要运行catkin_make编译一次消息定义文件。否则将无法生成可用的实现文件，Python将找不到你自定义的消息类型。

同样的道理，如果你修改完消息定义文件以后，忘记重新编译的话，那么你使用的消息依旧是老版本。

虽然这种机制听起来好像很麻烦，但是这其实是一种一劳永逸的方法，因为我们只需要定义一次消息，接着在所以ROS支持的编程语言中我们都可以使用这种消息。

ROS消息定义文件通常十分简短易懂，都是每一行一个类型名加一个变量名的格式。类型名是ROS的基础类型，或者来自其他包里定义的类型，或者是像Header这样的特殊类型。

像下面这样子的，就形成了一个简单的.msg文件。

	int8 demo1
	int32 demo2
	float32 demo3

应用.msg文件如下：

		float32 PosX             //用于点的三维坐标
		float32 PosY
		float32 PosZ
		float32 k1                //用于位姿的16个元素
		float32 k2
		float32 k3
		float32 k4
		float32 k5
		float32 k6
		float32 k7
		float32 k8
		float32 k9
		float32 k10
		float32 k11
		float32 k12
		float32 k13
		float32 k14
		float32 k15
		float32 k16

四.代码修改部分

①点的传输数目 RobotX.cpp

ORB_SLAM3_MultiRobot::PointPos msgPoint; 
	    if(_MapPointNum > MapPointNum)                                            //判断更新后的点数目与更新前的点的数目，若前者更大，则进入循环
	    {
	            for( i = MapPointNum ; i < _MapPointNum ; i++ )                       //开始初始化数据类型消息，循环输入
	            {
	                pos = OutSIdeVPMPs[i]->GetWorldPos();
	                msgPoint.PosX = pos.at(0);     //对应PointPos.msg文件里面的数据类型对应输入进去，这里是float32 PosX。
	                msgPoint.PosY = pos.at(1);     //对应float32 PosY
	                msgPoint.PosZ = pos.at(2);     //对应float32 PosZ
	                //话说这里的格式到底是咋写的[ msgPoint.PosX/Y/Z= pos.at(0/1/2)  ]，和PointPos.msg怎么对应书写？？子健NB,与上面一个定义有关，忽视了
	                pub.publish(msgPoint);
	                //cout << setprecision(3) << "msgPoint published" << msgPoint.PosX << " " 
	                //                        << msgPoint.PosY << " " << msgPoint.PosZ << endl;
	            }
    //    } ……

注意for循环的起始数值 i = MapPointNum ，根据理解，原理应该是这样：函数文件传输点每次更新之后，并不会再次重新开始传输，而是每次只会传输新的点。比如我第一次扫描识别到了10个点并传输，第二次在同样位置扫描识别到了20个点，但这次我只传输第11到第20个点，这样子使点的数目减少。

于是修改为 i= 0；使每次更新传输都是传输该阶段所能识别的所有点。

②位姿传输

Part 1 RobotX.cpp

//接上一小标题里的循环：
	ORB_SLAM3_MultiRobot::PointPos msgPoint; 
	const vector vpKFs = OutSidempAtlas->GetAllKeyframes(); //取自ORBSLAM2的MapDrawer.cc文件
	        for(size_t i=0; iGetPoseInverse().t();                //转置
	            msgPoint.k1 = pos.at(0);        //这里往下都是传输信息至.msg文件的对应数值，可翻看上面的.msg数据消息定义
	            msgPoint.k1 = Twc.at(0);
                msgPoint.k2 = Twc.at(1);
                msgPoint.k3 = Twc.at(2);
                msgPoint.k4 = Twc.at(3);
                msgPoint.k5 = Twc.at(4);
                msgPoint.k6 = Twc.at(5);
                msgPoint.k7 = Twc.at(6);
                msgPoint.k8 = Twc.at(7);
                msgPoint.k9 = Twc.at(8);
                msgPoint.k10 = Twc.at(9);
                msgPoint.k11= Twc.at(10);
                msgPoint.k12 = Twc.at(11);
                msgPoint.k13= Twc.at(12);
                msgPoint.k14 = Twc.at(13);
                msgPoint.k15 = Twc.at(14);
                msgPoint.k16 = Twc.at(15);
	
	        }

	//接上一小标题里的循环：
	ORB_SLAM3_MultiRobot::PointPos msgPoint; 
	const vector vpKFs = OutSidempAtlas->GetAllKeyframes(); //取自ORBSLAM2的MapDrawer.cc文件
	        for(size_t i=0; iGetPoseInverse().t();                //转置
	            msgPoint.k1 = pos.at(0);                              //这里往下都是传输信息至.msg文件的对应数值，可翻看上面的.msg数据消息定义
	            msgPoint.k2 = pos.at(1);
	            msgPoint.k3 = pos.at(2);
	            msgPoint.k4 = pos.at(3);
	            msgPoint.k5 = pos.at(4);
	            msgPoint.k6 = pos.at(5);
	            msgPoint.k7 = pos.at(6);
	            msgPoint.k8 = pos.at(7);
	            msgPoint.k9 = pos.at(8);
	            msgPoint.k10 = pos.at(9);
	            msgPoint.k11= pos.at(10);
	            msgPoint.k12 = pos.at(11);
	            msgPoint.k13= pos.at(12);
	            msgPoint.k14 = pos.at(13);
	            msgPoint.k15 = pos.at(14);
	            msgPoint.k16 = pos.at(15);
	
	        }

Part 2 MapPointMerge.cpp

	class Tmatrix                                 //定义好一个类，用于接收发布者数据
	{
	    public:
	
	    float Matrix1;
	    float Matrix2;
	    float Matrix3;
	    float Matrix4;
	    float Matrix5;
	    float Matrix6;
	    float Matrix7;
	    float Matrix8;
	    float Matrix9;
	    float Matrix10;
	    float Matrix11;
	    float Matrix12;
	    float Matrix13;
	    float Matrix14;
	    float Matrix15;
	    float Matrix16;
	
	};
	
	void AddTMatrixToVector(const ORB_SLAM3_MultiRobot::PointPos::ConstPtr &msg)
	{
	
	    static TMatrix* _pose;                //作为回调函数一部分获取发布者的数据，Twc元素来源
	    _pose = (TMatrix*)malloc(sizeof(TMatrix));
	    _pose->Matrix1 = msg->k1;                                     //用刚定义的类来对应.msg变量进行接收
	    _pose->Matrix2 = msg->k2;
	    _pose->Matrix3 = msg->k3;
	    _pose->Matrix4 = msg->k4;
	    _pose->Matrix5 = msg->k5;
	    _pose->Matrix6 = msg->k6;
	    _pose->Matrix7 = msg->k7;
	    _pose->Matrix8 = msg->k8;
	    _pose->Matrix9 = msg->k9;
	    _pose->Matrix10 = msg->k10;
	    _pose->Matrix11 = msg->k11;
	    _pose->Matrix12 = msg->k12;
	    _pose->Matrix13 = msg->k13;
	    _pose->Matrix14 = msg->k14;
	    _pose->Matrix15 = msg->k15;
	    _pose->Matrix16 = msg->k16;
	    TMatrixVectors.push_back(_pose);          //将_pose塞进去 TMatrixVectors 向量中
	}
	
	    int TMatrixNum= TMatrixVectors.size();  
	    //定义Twc 4*4，并输入16个元素
	    for(  int i=0  ;  i<  TMatrixNum ;  i++  ){      //定义并输入Twc
	        float Twc[4][4]={TMatrixVectors[i]->Matrix1,TMatrixVectors[i]->Matrix2,TMatrixVectors[i]->Matrix3,TMatrixVectors[i]->Matrix4,
	        TMatrixVectors[i]->Matrix5,TMatrixVectors[i]->Matrix6,TMatrixVectors[i]->Matrix7,TMatrixVectors[i]->Matrix8,TMatrixVectors[i]->Matrix9,
	        TMatrixVectors[i]->Matrix10,TMatrixVectors[i]->Matrix11,TMatrixVectors[i]->Matrix12,TMatrixVectors[i]->Matrix13,TMatrixVectors[i]->Matrix14,
	        TMatrixVectors[i]->Matrix15,TMatrixVectors[i]->Matrix16};
	        const float w=0.05;
	        const float h=w*0.75;
	        const float z=w*0.6;
	        cv::Mat  _Twc=cv::Mat(4,4,CV_32FC1,Twc);
	        glPushMatrix();                 //画关键帧，函数来源于ORBSLAM2的MapDrawer.cc文件
	        glMultMatrixf(_Twc.ptr(0));
	        //设置绘制图形时线的宽度
	        glLineWidth(3);
	        //设置当前颜色为蓝色(关键帧图标显示为蓝色)
	        glColor3f(0.0f,0.0f,1.0f);
	        //用线将下面的顶点两两相连
	        glBegin(GL_LINES);
	        glVertex3f(0,0,0);
	        glVertex3f(w,h,z);
	        glVertex3f(0,0,0);
	        glVertex3f(w,-h,z);
	        glVertex3f(0,0,0);
	        glVertex3f(-w,-h,z);
	        glVertex3f(0,0,0);
	        glVertex3f(-w,h,z);
	        glVertex3f(w,h,z);
	        glVertex3f(w,-h,z);
	        glVertex3f(-w,h,z);
	        glVertex3f(-w,-h,z);
	        glVertex3f(-w,h,z);
	        glVertex3f(w,h,z);
	        glVertex3f(-w,-h,z);
	        glVertex3f(w,-h,z);
	        glEnd();
	
	        glPopMatrix();
	        }
	
	        // 运行帧循环以推进窗口事件
	        pangolin::Finishframe();
	    }

2021年11月6日

星期六- 复现第二阶段点与关键帧

一.概述
在师兄给的初始文件上进行修改，ROS机器人添加SLAM功能。

根据师兄要求，首先是对传输融合地图点的数目进行修改，其次是给融合地图添加一个”画关键帧“的功能。

二.ROS话题传输

四.代码修改部分

五、效果图

七、小喝彩
完成修改，其实挺难的，作为一个小幸运日，刚好迎来LOL总决赛！

和同学聚在宿舍一起看比赛，一起撕心裂肺的吼叫。

最后终于迎来胜利！EDGNB!我们又是冠军！

来张央视新闻牌面帅图~

Python相关栏目本月热门文章

2021年11月6日

星期六- 复现第二阶段 点与关键帧

一.概述 在师兄给的初始文件上进行修改，ROS机器人添加SLAM功能。 根据师兄要求，首先是对传输融合地图点的数目进行修改，其次是给融合地图添加一个”画关键帧“的功能。

二.ROS话题传输

四.代码修改部分

五、效果图

七、小喝彩 完成修改，其实挺难的，作为一个小幸运日，刚好迎来LOL总决赛！ 和同学聚在宿舍一起看比赛，一起撕心裂肺的吼叫。 最后终于迎来胜利！EDGNB!我们又是冠军！ 来张央视新闻牌面帅图~

Python相关栏目本月热门文章

星期六- 复现第二阶段点与关键帧

一.概述
在师兄给的初始文件上进行修改，ROS机器人添加SLAM功能。

根据师兄要求，首先是对传输融合地图点的数目进行修改，其次是给融合地图添加一个”画关键帧“的功能。

七、小喝彩
完成修改，其实挺难的，作为一个小幸运日，刚好迎来LOL总决赛！

和同学聚在宿舍一起看比赛，一起撕心裂肺的吼叫。

最后终于迎来胜利！EDGNB!我们又是冠军！

来张央视新闻牌面帅图~