使用ros发布UVC相机和串口IMU数据

１．目的：为了可以标定普通USB相机和固定在相机上的外置IMU的外参,我希望通过ROS获取更高分辨率和更高频率的图像数据,并且可以将图像和imu的topic发布出来,直接使用rosbag record录制话题数据,写入bag文件,这样获得的bag文件直接可以用于相机和IMU的外参标定, 标定工具是kalibr.

2. 为了达到上述目的,我首先是完成使用ros发布出来从串口获取的imu数据,目前获取的频率是200hz,也是从网上找到的一个ros中串口通信的小demo,     参考   https://blog.csdn.net/tansir94/article/details/81357612  和   https://blog.csdn.net/xinmei4275/article/details/85040164?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase

3. 我安装了minicom  串口小工具

sudo apt-get install minicom
sudo minicom -D /dev/ttyUSB0 -b 230400 -H -w
-D 设置串口
-b 设置波特率
-H 设置十六进制显示
-w 自动换行

我的代码如下

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <serial/serial.h> //ROS已经内置了的串口包
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
serial::Serial ser; //声明串口对象sensor_msgs::Imu imu_data;
//回调函数
void write_callback(const std_msgs::String::ConstPtr &msg) {ROS_INFO_STREAM("Writing to serial port" << msg->data);ser.write(msg->data); //发送串口数据
}
int main(int argc, char **argv) {//初始化节点ros::init(argc, argv, "serial_example_node");//声明节点句柄ros::NodeHandle nh;//订阅主题，并配置回调函数ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback);//发布主题ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);ros::Publisher IMU_read_pub =nh.advertise<sensor_msgs::Imu>("imu_data", 1000);// ros::Publisher Image_read_pub =
//       nh.advertise<sensor_msgs::>("imu_data", 1000);try {//设置串口属性，并打开串口ser.setPort("/dev/ttyUSB0");ser.setBaudrate(230400);serial::Timeout to = serial::Timeout::simpleTimeout(1000);ser.setTimeout(to);ser.open();} catch (serial::IOException &e) {ROS_ERROR_STREAM("Unable to open port ");return -1;}//检测串口是否已经打开，并给出提示信息if (ser.isOpen()) {ROS_INFO_STREAM("Serial Port initialized");} else {return -1;}//指定循环的频率ros::Rate loop_rate(400);while (ros::ok()) {unsigned char data_size;/*! Return the number of characters in the buffer. */// available()
//     while((data_size = ser.available()) < 11){//     } if (data_size = ser.available()){unsigned char tmpdata[data_size];ser.read(tmpdata, data_size);// std::cout << "data_size: " << data_size << std::endl;for (int i = 0; i < data_size; i++) {if (tmpdata[i] == 0x55) {switch (tmpdata[i + 1]) {case 0x51:imu_data.header.stamp = ros::Time::now();imu_data.header.frame_id = "base_link";imu_data.linear_acceleration.x =((short)(tmpdata[3 + i] << 8 | tmpdata[2 + i])) / 32768.0 * 4 * 9.7803;imu_data.linear_acceleration.y =((short)(tmpdata[5 + i] << 8 | tmpdata[4 + i])) / 32768.0 * 4 * 9.7803;imu_data.linear_acceleration.z =((short)(tmpdata[7 + i] << 8 | tmpdata[6 + i])) / 32768.0 * 4 * 9.7803;std::cout<<"acc: "<<std::setprecision(16)<<imu_data.header.stamp<<" " <<imu_data.linear_acceleration.x<<" "<<imu_data.linear_acceleration.y<<" "<<imu_data.linear_acceleration.z<<std::endl<<std::endl;break;case 0x52:imu_data.angular_velocity.x =((short)(tmpdata[3 + i] << 8 | tmpdata[2 + i])) / 32768.0 * 500;imu_data.angular_velocity.y =((short)(tmpdata[5 + i] << 8 | tmpdata[4 + i])) / 32768.0 * 500;imu_data.angular_velocity.z =((short)(tmpdata[7 + i] << 8 | tmpdata[6 + i])) / 32768.0 * 500;//ROS_INFO_STREAM("imu_data: " << imu_data);IMU_read_pub.publish(imu_data);std::cout<<"gyr: "<<std::setprecision(16)<<imu_data.header.stamp<<" "<<imu_data.angular_velocity.x <<" "<<imu_data.angular_velocity.y <<" "<<imu_data.angular_velocity.z <<std::endl;break; }}}}//     if (ser.available()) {//       // ROS_INFO_STREAM("Reading from serial port\n");//       //读到的是string类型的,//       std_msgs::String result;//       result.data = ser.read(ser.available());//       ROS_INFO_STREAM("Read: " << result.data);//       std::cout << "result.data: " << result.data << std::endl <<//       std::endl;//       // read_pub.publish(result);//     }//处理ROS的信息，比如订阅消息,并调用回调函数ros::spinOnce();loop_rate.sleep();}
}

CMakeLists.txt的内容

cmake_minimum_required(VERSION 2.8.3)
project(serialPort)find_package(catkin REQUIRED COMPONENTSroscppserialstd_msgs
)catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES serialPortCATKIN_DEPENDS roscpp serial std_msgs
#  DEPENDS system_lib
)###########
## Build ##
############# Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
#include${catkin_INCLUDE_DIRS}
)add_executable(serialPort src/serialPort.cpp)add_dependencies(serialPort ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})target_link_libraries(serialPort${catkin_LIBRARIES}
)

4. 获取uvc相机的图像

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <stdio.h>
using namespace cv;int main(int argc, char **argv) {ros::init(argc, argv, "image_publisher");ros::NodeHandle nh;image_transport::ImageTransport it(nh);image_transport::Publisher pub0 = it.advertise("camera/left", 0);image_transport::Publisher pub1 = it.advertise("camera/right", 1);//测试出来我的小觅相机是单ID相机cv::VideoCapture cap(0);cap.set(CV_CAP_PROP_FRAME_WIDTH, 2560);cap.set(CV_CAP_PROP_FRAME_HEIGHT, 720);cap.set(CV_CAP_PROP_FPS, 60);// cap.set(CV_CAP_PROP_FRAME_WIDTH, 1280);// cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);// cap.set(CV_CAP_PROP_FPS, 60);if (!cap.isOpened()) {ROS_INFO("cannot open video device0\n");return -1;}cv::Mat frame, frame_G, frame_L, frame_R;sensor_msgs::ImagePtr msg0;sensor_msgs::ImagePtr msg1;sensor_msgs::ImagePtr msg;ros::Rate loop_rate(30);while (nh.ok()) {cap >> frame;ros::Time time_now = ros::Time::now();cv::cvtColor(frame, frame_G, cv::COLOR_BGR2GRAY);frame_L = frame_G(cv::Rect(0, 0, 1280, 720));frame_R = frame_G(cv::Rect(1280, 0, 1280, 720));// frame_L = frame_G(cv::Rect(0, 0, 640, 480));// frame_R = frame_G(cv::Rect(640, 0, 640, 480));// cv::imshow("frame_L", frame_L);// cv::waitKey(2);// cv::imshow("frame_R", frame_R);// cv::waitKey(2);// cv::imshow("frame", frame);// cv::waitKey(2);// cv::imshow("frame_G", frame_G);// cv::waitKey(2);if (!frame_L.empty()) {msg0 =cv_bridge::CvImage(std_msgs::Header(), "mono8", frame_L).toImageMsg();msg0->header.stamp = time_now;pub0.publish(msg0);}if (!frame_R.empty()) {msg1 =cv_bridge::CvImage(std_msgs::Header(), "mono8", frame_R).toImageMsg();msg1->header.stamp = time_now;pub1.publish(msg1);}ROS_INFO("Publishing camera/left camera/right ROS topic MSG!! ");ros::spinOnce();loop_rate.sleep();}
}

CMakeLists.txt文件内容如下

cmake_minimum_required(VERSION 2.8.3)
project(imgReader)find_package(catkin REQUIRED COMPONENTSsensor_msgscv_bridgeroscppstd_msgsimage_transport
)
find_package(OpenCV REQUIRED)#
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES imgReaderCATKIN_DEPENDS sensor_msgs cv_bridge roscpp std_msgs image_transport
#  DEPENDS system_lib
)###########
## Build ##
############# Specify additional locations of header files
## Your package locations should be listed before other locationsinclude_directories(
# include${catkin_INCLUDE_DIRS}${OpenCV_INCLUDE_DIRS}
)add_executable(imgReader src/imgReader.cpp)target_link_libraries(imgReader${catkin_LIBRARIES}${OpenCV_LIBRARIES}
)
add_dependencies(imgReader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

5. 设置rviz显示图像和Imu数据

首先设置launch文件

<launch><node pkg="serialPort" type="serialPort" name="serialPort" required="true" output="screen"><param name="port" value="/dev/ttyUSB0"/></node><node pkg="imgReader" type="imgReader" name="imgReader" required="true" output="screen"><param name="port" value="/dev/video0"/></node><!-- 在rviz中显示-->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find imgReader)/config/rviz/get_imu_image.rviz" required="true" /></launch>

如果不设置launch文件,也可以分别发布两个节点的信息.

terminal 1
roscoreterminal 2
source ./devel/setup.bash
rosrun serialPort serialPortterminal 3
source ./devel/setup.bash
rosrun imgReader imgReader

参考这篇文章设置rviz的配置文件  https://blog.csdn.net/weixin_44684139/article/details/104416690 和 https://blog.csdn.net/xinmei4275/article/details/85040164?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase

首先是运行你的node,使得图像和imu数据的topics都发布出来,使用rostopic list查看当前正在发布的topic

然后打开rviz窗口界面

rosrun rviz rviz

然后添加Imu和image的topics

最后保存rviz配置到相应的路径.然后在上面的launch文件最后加上rviz配置文件,这样,当roslaunch launch文件时,就可以同时加载rviz视图窗口了.

就像下图这样.

 

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/252429.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

API自动化测试利器——Postman

API自动化测试利器——Postman

自从开始做API开发之后&#xff0c;我就在寻找合适的API测试工具。一开始不是很想用Chrome扩展&#xff0c;用的WizTools的工具&#xff0c;后来试过一次Postman之后就停不下来了&#xff0c;还买了付费的Jetpacks。推出Team Sync Beta之后我又把这个工具推广给团队&#xff0c…
阅读更多...
云原生实践之 RSocket 从入门到落地：Servlet vs RSocket

云原生实践之 RSocket 从入门到落地：Servlet vs RSocket

技术实践的作用在于&#xff1a;除了用于构建业务&#xff0c;也是为了验证某项技术或框架是否值得大规模推广。 本期开始&#xff0c;我们推出《RSocket 从入门到落地》系列文章&#xff0c;通过实例和对比来介绍RSocket。主要围绕RSocket如何实现Polyglot RPC、Service Regis…
阅读更多...
制作.sens数据集跑通bundlefusion

制作.sens数据集跑通bundlefusion

1. 主要参考这篇博客实现 https://blog.csdn.net/Wuzebiao2016/article/details/94426905 2. 首先就是将自己采集的RGBD图像的保存格式向Bundlefusion需要的格式对齐&#xff0c;如彩色图的命名格式是frame-000000.color.png&#xff0c;深度图的命名规则是frame-000000.depth…
阅读更多...
python之moviepy库的安装与使用

python之moviepy库的安装与使用

目的&#xff1a;因为需要保存一个大大的.mp4视频&#xff0c;以防过程中设备出现异常导致整个长长的视频无法正常保存&#xff0c;所以采用分段保存视频的方式&#xff0c;每500帧保存一段&#xff0c;然后再将视频合到一起&#xff0e;最近刚开始学习python,发现python真的很…
阅读更多...
使用iai_kinect2标定kinectV2相机

使用iai_kinect2标定kinectV2相机

实验背景&#xff1a;因为需要制作bundlefusion需要的数据集&#xff0c;所以需要使用kinectV2相机获取rgbd图像&#xff0c;年前的时候在我的笔记本上安装了libfreenect2库和iai_kinect2&#xff0c;标定过一次kinecv2相机&#xff0c;然后使用kinectv2相机实时获取的图像实现…
阅读更多...
Linux下配置tomcat+apr+native应对高并发

Linux下配置tomcat+apr+native应对高并发

摘要&#xff1a;在慢速网络上Tomcat线程数开到300以上的水平&#xff0c;不配APR&#xff0c;基本上300个线程狠快就会用满&#xff0c;以后的请求就只好等待。但是配上APR之后&#xff0c;Tomcat将以JNI的形式调用Apache HTTP服务器的核心动态链接库来处理文件读取或网络传输…
阅读更多...
Firefox 66 将阻止自动播放音频和视频

Firefox 66 将阻止自动播放音频和视频

百度智能云 云生态狂欢季 热门云产品1折起>>> 当我们点击一个链接&#xff0c;或者打开新的浏览器选项卡时&#xff0c;浏览器就开始自动播放视频和声音&#xff0c;这是一件十分烦人的事。Chrome 浏览器早已对这些行为下手了&#xff0c;现在 Firefox 也明确表示要…
阅读更多...
Windows 10 关闭Hyper-V

Windows 10 关闭Hyper-V

以管理员身份运行命令提示符 关闭 bcdedit /set hypervisorlaunchtype off 启用 bcdedit / set hypervisorlaunchtype auto 禁用DG 转载于:https://www.cnblogs.com/Robbery/p/8397767.html
阅读更多...
bundlefusion论文阅读笔记

bundlefusion论文阅读笔记

4. 全局位姿对齐(glob pose alignment) 输入系统的是使用消费级的传感器获取的RGBD数据流&#xff0c;并且保证这些数据中的彩色图像和深度图像是时间和空间上都对齐的。图像分辨率是640x480,频率是30hz。我们的目的就是要找到frames之间的3D对应&#xff0c;然后根据这些对应…
阅读更多...
IOC和DI的区别详解

IOC和DI的区别详解

IOC 是英文inversion of control的缩写&#xff0c;意思是控制反转DI 是英文Dependency Injection的缩写&#xff0c;意思是依赖注入 下面用一个简单的例子来描述一下IOC和DI的关系 先看下总结&#xff1a; 依赖注入(DI)和控制反转(IOC)是从不同的角度的描述的同一件事情&#…
阅读更多...
TOMCAT启动到一半停止如何解决

TOMCAT启动到一半停止如何解决

当你的项目过大的时候&#xff0c;往往会导致你的TOMCAT启动时间过长&#xff0c;启动失败&#xff0c;遇到该情况可以试一下下面两招&#xff1a; TOmcat启动到一半的时候停止了&#xff0c;以下原因&#xff1a; 1、 tomcat启动时间超过了设置时间&#xff1a; 解决办法&…
阅读更多...
python中将四元数转换为旋转矩阵

python中将四元数转换为旋转矩阵

在制作bundlefusion时,想测试TUM数据集,并且将groundtruth写入到数据集中,TUM中给定的groundtruth中的旋转是使用四元数表示的,而bundlefusion中需要SE3的形式,所以我需要首先将四元数转换为旋转矩阵,然后再将其与平移向量合并在一起,因为我之前关于生成bundlefusion数据集写了…
阅读更多...
19、Fragment

19、Fragment

一、Fragment 1.1、fragment介绍 fragment的出现是为了同时适应手机和平板&#xff0c;可以将其看做Activity的组成部分&#xff0c;甚至Activity界面完全由不同的Fragment组成&#xff0c;它拥有自己的生命 周期和接收、处理用户的事件&#xff0c;更为重要的是&#xff0c;可…
阅读更多...
将TUM数据集制作成BundleFusion数据集

将TUM数据集制作成BundleFusion数据集

在上一篇文章中,我写到了如何将TUM数据生成BundleFusion所需要的数据集,生成的数据集如下图中所示.并且是将每一组数据的groundtruth.txt中的位姿数据写如到这里的pose文件中,作为每一帧图像的先验位姿. 今天我便将生成的数据集转换为了.sens格式,然后运行bundlefusion算法,第…
阅读更多...
mysql盲注学习-1

mysql盲注学习-1

mysql: 1.left() //left()函数 left(a,b)从左侧截取a,的b位 2.mid() //mid()函数 参数 描述 column_name 必需。要提取字符的字段。 start 必需。规定开始位置&#xff08;起始值是 1&#xff09;。 length 可选。要返回的字符数。如果省略&#xff0c;则 MID() 函数…
阅读更多...
二分学习笔记

二分学习笔记

写在前面 二分是一种常用且非常精妙的算法&#xff0c;常常是我们解决问题的突破口。二分的基本用途是在单调序列或单调函数中做查找。因此当问题的答案具有单调性时&#xff0c;就可以通过二分把求解转化为判定。进一步地&#xff0c;我们还可以通过三分法解决单调函数的极值以…
阅读更多...
ConcurrentHashMap 解读

ConcurrentHashMap 解读

初始化&#xff1a; 问题&#xff1a;如何当且仅只有一个线程初始化table 1 private final Node<K,V>[] initTable() {2 Node<K,V>[] tab; int sc;3 while ((tab table) null || tab.length 0) {4 if ((sc sizeCtl) < 0)5 …
阅读更多...
BundleFusion那些事儿

BundleFusion那些事儿

背景&#xff1a;前面几篇博客中写了很多关于BundleFusion的东西&#xff0c;主要包括bundlefusion的论文阅读笔记&#xff0c;.sens数据集的生成等&#xff0c;经过最近几天的工作&#xff0c;我对bundlefusion又有了新的技术积累&#xff0c;在这里整理一下&#xff0c;也算是…
阅读更多...
Oracle Study之--Oracle 11gR2通过RMAN克隆数据库

Oracle Study之--Oracle 11gR2通过RMAN克隆数据库

Oracle Study之--Oracle 11gR2通过RMAN克隆数据库Purpose of Database Duplication A duplicate database is useful for a variety of purposes, most of which involve testing. You can perform the following tasks in a duplicate database: Test backup and recovery pro…
阅读更多...
手机录音ogg格式怎么转换mp3

手机录音ogg格式怎么转换mp3

Ogg这种音频格式刚出来的时候大家是非常热爱的&#xff0c;但是随着时代的发展&#xff0c;这种音频格式已经已经被取代了&#xff0c;现在呢走在音频格式前端的是MP3格式&#xff0c;这是大家都比较熟悉的&#xff0c;但是我们还是会经常下载到ogg这种格式的音频&#xff0c;就…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023