REMODE+ORBSLAM运行配置(2) REMODE和编译后的ORB ros工程利用节点实现通讯

原创 2017年01月03日 09:16:49

将ORB编译成ROS工程后,就可以利用ROS的节点通信实现ORB和REMODE的数据传输。

步骤:

1 仿照SVO-REMODE的编译方式。安装googletest 和rpg_open_remode至~/catkin_ws/src 目录下。

2 修改 ORB的主函数,以便发送数据。

2.1 加入头文件

#include<vikit/file_reader.h>      
#include<vikit/params_helper.h> 
#include<vikit/camera_loader.h>
//以上几个头文件是从程序外从参数用的,需要下载vikit源码
#include<cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
//以上3个头文件是发送图片用的
#include<ros/ros.h>
#include<ros/package.h>
#include"std_msgs/String.h"
#include"sstream"
#include"DenseInput.h"//这个头文件 说明了传输数据的结构,需要从svo工程svo_msgs里的拷出来。
#include <Converter.h>//位姿矩阵转四元素
#include <pangolin/pangolin.h>
#include <iomanip>
2.2 在main函数中完成通信

以下修改在main函数中

开启ros节点,vk::getParam才能找到launch文件的参数

ros::init(argc,argv,"ORB_SLAM");
   ros::NodeHandle SendMessage;
// 发布
   ros::Publisher pub_dense_=SendMessage.advertise<svo_msgs::DenseInput>("dense_input",10);
   //利用vikit读取参数
    const string dataset_path(vk::getParam<std::string>("ORB_SLAM/Img_Path"));
    const string CamPara_Path(vk::getParam<std::string>("ORB_SLAM/CamPara_Path"));
    const string BOW_Path(vk::getParam<std::string>("ORB_SLAM/BOW_Path"));
 
发送数据:每执行一次slam,发送一次数据。数据包括 

1 frame的id

2 左图

3 四元数格式的位姿

4 当前场景的最大最小深度。

/////////////////////////////////////////////////////////与REMODE通信//////////////////////////////

	svo_msgs::DenseInput msg;
	msg.header.stamp=ros::Time(20);//svo中设置20可以正常运行
	msg.header.frame_id="/world";
	msg.frame_id=framecnt;
	cv_bridge::CvImage img_msg;
	img_msg.header.stamp=msg.header.stamp;
	img_msg.header.frame_id="camera";
	img_msg.image=imRGB;

	img_msg.encoding = sensor_msgs::image_encodings::MONO8;
	msg.image = *img_msg.toImageMsg();

	double min_z = std::numeric_limits<double>::max();
	double max_z = std::numeric_limits<double>::min();
	SLAM.mpTracker->getSceneDepth(SLAM.mpTracker->mCurrentFrame,max_z,min_z);
	cout<<"min_z-----------------------------------------  "<<min_z<<endl;
	cout<<"max_z-------------------------------------------  "<<max_z<<endl;
         msg.min_depth=(float)min_z;
         msg.max_depth=(float)max_z;

	cv::Mat  TWC=SLAM.mpTracker->mCurrentFrame.mTcw.inv();
	cv::Mat RWC=TWC.rowRange(0,3).colRange(0,3);
	cv::Mat tWC=TWC.rowRange(0,3).col(3);
	vector<float> q=ORB_SLAM2::Converter::toQuaternion(RWC);



	    msg.pose.position.x = tWC.at<float>(0,0);
	    msg.pose.position.y = tWC.at<float>(1,0);
	    msg.pose.position.z = tWC.at<float>(2,0);
	    msg.pose.orientation.w = q[3];//q.w();
	    msg.pose.orientation.x = q[0];//q.x();
	    msg.pose.orientation.y = q[1];//q.y();
	    msg.pose.orientation.z = q[2];//q.z();
	    pub_dense_.publish(msg);
////////////////////////////////////////////////////////////////////////////////////////////////////////

opencv的cv::Mat  和ros中的sensor_msgs/Image 需要cv_bridge::CvImage  做个转换。

用法:http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages

3 仿照

ubuntu下通过命令打开多个终端并在相应终端执指令

写一个脚本,目的是在运行工程的时候方便,快捷。脚本内容为

gnome-terminal -x bash -c "roscore"
gnome-terminal -x bash -c "rosrun rviz rviz -d /home/baohua/project/SLAM/REMODE_ORB/catkin_ws/src/rpg_open_remode/open_remode.rviz  "
gnome-terminal -x bash -c " source '/home/baohua/project/SLAM/REMODE_ORB/catkin_ws/devel/setup.sh';roslaunch ORB_SLAM MH01.launch "
gnome-terminal -x bash -c " source '/home/baohua/project/SLAM/REMODE_ORB/catkin_ws/devel/setup.sh';roslaunch rpg_open_remode remode_EuRoC.launch "

至此,运行脚本就可以运行REMODE+ORBSLAM工程了。

版权声明:本文为博主原创文章,未经博主允许不得转载。

ORB_SLAM软件包的安装及简单测试

本博文简单记录了基于Ubuntu 14.14 LTS 和 ROS Ingigo 环境的 ORB_SLAM 功能包的安装与使用,并提供了一个测试安装是否成功的例样。...
  • Dreamer_lhs
  • Dreamer_lhs
  • 2016年05月02日 18:00
  • 4035

ORB SLAM2 ROS实例 单目摄像头

ORB SLAM2 USB摄像头实验环境ubuntu
  • xmy306538517
  • xmy306538517
  • 2017年03月02日 14:15
  • 2203

在ROS中使用usb摄像头跑ORB SLAM2

摘要(假装这是论文):由于最近在做orb_slam2相关,将这几天的实验过程总结记录,本文主要为傻瓜式记录运行代码的过程,以及中间遇到的问题及解决方法,截止至发文前,只是把单目Mono的功能实现,暂时...
  • Goding_learning
  • Goding_learning
  • 2016年10月27日 22:52
  • 4390

REMODE+ORBSLAM运行配置(1) 把ORB编译成ROS工程

原版的ORBSLAM是一个cmake工程,由于项目的需要,我们要在其他ROS工程(简称A)和ORB工程进行通讯,即需要将这两个工程联编,这就有两个办法: 方法一:把A工程修改成一个cmake工程,然后...
  • zhubaohua_bupt
  • zhubaohua_bupt
  • 2016年12月27日 11:49
  • 744

SVO-REMODE编译运行方法

/////////////////////////////////////////////////////////////////// 作者:朱保华 REMODE+SVO编译运行方法。 ///////...
  • zhubaohua_bupt
  • zhubaohua_bupt
  • 2016年12月23日 16:46
  • 1553

ORB-SLAM2安装与运行(非ROS环境下)

这是在没有使用ROS的情况下运行跑的论文:ORB-SLAM2: an Open-Source SLAM System for Monocular,Stereo and RGB-D Cameras提供的...
  • zhuquan945
  • zhuquan945
  • 2017年05月13日 12:52
  • 460

在ROS indigo下运行ORB-SLAM2

在ROS indigo下运行ORB-SLAM2
  • xingzhou1212
  • xingzhou1212
  • 2017年03月09日 09:37
  • 483

编译安装及运行单目ORBSLAM2在Ubuntu14.04

版权声明:本文为博主原创文章,未经博主允许不得转载。 前言 ORBSLAM2是一个非常适合SLAM入门学习的开源工程。它支持单目、双目、RGB-D使用,可以计算摄像机的轨迹,并且重建稀疏的...
  • luopeiyuan1990
  • luopeiyuan1990
  • 2017年02月09日 11:28
  • 1649

Ubuntu 16 ORB_SLAM2使用KinectV2在ROS上运行总结

基本配置:Ubuntu 16.04 LTS ,ROS::Indigo(kinetic failed!),Opencv2.4.13,主机只有集成显卡,还有KinectV2...
  • Fate_fjh
  • Fate_fjh
  • 2016年11月25日 10:15
  • 4318

Ubuntu 16.04+ROS+ORB-SLAM2配置以及相关问题

最近尝试在ubuntu 16.04下配置ORB-SLAM2,现将相关过程以及遇到的问题进行记录,给大家做个参考。 网上有很多ubuntu 14.04下配置ORB-SLAM的文章,和我们这个会有区别...
  • wys422
  • wys422
  • 2017年07月22日 13:42
  • 347
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:REMODE+ORBSLAM运行配置(2) REMODE和编译后的ORB ros工程利用节点实现通讯
举报原因:
原因补充:

(最多只允许输入30个字)