使用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…
阅读更多...
gcc,cc,g++,CC的区别

gcc,cc,g++,CC的区别

***gcc是C编译器&#xff1b; ***g是C编译器&#xff1b; ***linux下cc一般是一个符号连接&#xff0c;指向gcc ***gcc说明 1.gcc编译常用格式&#xff1a; gcc C源文件 -o 目标文件 或 gcc -o 目标文件 C源文件 或 gcc C源文件 最后一种情况产生的目标文件默认为a.out 2.gcc…
阅读更多...
云原生实践之 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…
阅读更多...
用ul li实现边框重合并附带鼠标经过效果

用ul li实现边框重合并附带鼠标经过效果

边框重合这个效果并不难&#xff0c;只是我们没有真正的动手做过而已&#xff0c;下面让我们来谈谈用ul li如何实现边框重合&#xff0c;并附带鼠标经过效果 1 <!DOCTYPE html>2 <html lang"zh-CN">3 4 <head>5 <title></title>6 …
阅读更多...
hive 中窗口函数row_number,rank,dense_ran,ntile分析函数的用法

hive 中窗口函数row_number,rank,dense_ran,ntile分析函数的用法

https://www.cnblogs.com/wujin/p/6051768.html转载于:https://www.cnblogs.com/0xcafedaddy/p/8385476.html
阅读更多...
python之moviepy库的安装与使用

python之moviepy库的安装与使用

目的&#xff1a;因为需要保存一个大大的.mp4视频&#xff0c;以防过程中设备出现异常导致整个长长的视频无法正常保存&#xff0c;所以采用分段保存视频的方式&#xff0c;每500帧保存一段&#xff0c;然后再将视频合到一起&#xff0e;最近刚开始学习python,发现python真的很…
阅读更多...
oracle 之 安装后pl/sql登录报ora-12154

oracle 之 安装后pl/sql登录报ora-12154

这个问题一开始困扰了很久。 查的资料是复制一小段代码到tnsnames.ora中 SID名 (DESCRIPTION (ADDRESS (PROTOCOL TCP)(HOST localhost)(PORT 1522)) (CONNECT_DATA (SERVER DEDICATED) (SERVICE_NAME SID名) ) 注意SID名前面不能有任何其他字符&…
阅读更多...
如何避免表单重复提交

如何避免表单重复提交

客户端方案 禁掉提交按钮。 表单提交后使用Javascript使提交按钮disable。这种方法防止心急的用户多次点击按钮。但有个问题&#xff0c;如果客户端把Javascript给禁止掉&#xff0c;这种方法就无效了。 使用Post/Redirect/Get模式 在提交后执行页面重定向&#xff0c;这就是所…
阅读更多...
十六进制转八进制

十六进制转八进制

【问题描述】 问题描述 给定n个十六进制正整数&#xff0c;输出它们对应的八进制数。 输入格式 输入的第一行为一个正整数n &#xff08;1<n<10&#xff09;。 接下来n行&#xff0c;每行一个由0~9、大写字母A~F组成的字符串&#xff0c;表示要转换的十六进制正整数&…
阅读更多...
使用iai_kinect2标定kinectV2相机

使用iai_kinect2标定kinectV2相机

实验背景&#xff1a;因为需要制作bundlefusion需要的数据集&#xff0c;所以需要使用kinectV2相机获取rgbd图像&#xff0c;年前的时候在我的笔记本上安装了libfreenect2库和iai_kinect2&#xff0c;标定过一次kinecv2相机&#xff0c;然后使用kinectv2相机实时获取的图像实现…
阅读更多...
tar只解压tar包中某个文件

tar只解压tar包中某个文件

如果tar包很大&#xff0c;而只想解压出其中某个文件。方法如下&#xff1a; 只想解压出Redis-1.972.tar 中的Changes文件&#xff0c;来查看有哪些更改。 [rootuplooking]# tar -tf Redis-1.972.tar Redis-1.972…
阅读更多...
扎克伯格的中文夜：想要成功就不能放弃

扎克伯格的中文夜：想要成功就不能放弃

10月23日消息。虽然并不太流畅。昨天马克•扎克伯格依旧用中文与清华经管学院主持人完毕了一场对话&#xff1b;在对话中&#xff0c;这位Facebook创始人兼首席运行官阐述了自己学习中文的原因&#xff1a;想要和太太&#xff08;普里西拉•陈&#xff09;的家人交流&#xff1…
阅读更多...
python将ros下bag文件的所有topic解析为csv格式

python将ros下bag文件的所有topic解析为csv格式

背景&#xff1a;最近在制作kimera的数据集&#xff0c;尤其是运行semantic模块所需要的bag文件中有很多topic&#xff0c;但是很多不知道topic中装的是什么数据&#xff0c;及其格式&#xff0c;所以我就想着怎么可以将bag中的topic都解析数来&#xff0c;这样就能知道bag中都…
阅读更多...
十九. Python基础(19)--异常

十九. Python基础(19)--异常

十九. Python基础(19)--异常 1 ● 捕获异常 if VS异常处理: if是预防异常出现, 异常处理是处理异常出现 异常处理一般格式: try: <...............> #可能得到异常的语句 except <.......>: #捕获是哪种异常 <...............> #出现异常的处理方…
阅读更多...
洛谷1052——过河（DP+状态压缩）

洛谷1052——过河（DP+状态压缩）

题目描述 在河上有一座独木桥&#xff0c;一只青蛙想沿着独木桥从河的一侧跳到另一侧。在桥上有一些石子&#xff0c;青蛙很讨厌踩在这些石子上。由于桥的长度和青蛙一次跳过的距离都是正整数&#xff0c;我们可以把独木桥上青蛙可能到达的点看成数轴上的一串整点&#xff1a;0…
阅读更多...
Tensorflow学习教程------tfrecords数据格式生成与读取

Tensorflow学习教程------tfrecords数据格式生成与读取

首先是生成tfrecords格式的数据&#xff0c;具体代码如下&#xff1a; #coding:utf-8import os import tensorflow as tf from PIL import Imagecwd os.getcwd() 此处我加载的数据目录如下&#xff1a; bt -- 14018.jpg14019.jpg14020.jpgnbt -- 1_ddd.jpg1_dsdfs.jpg1_dfd.…
阅读更多...
ROS获取KinectV2相机的彩色图和深度图并制作bundlefusion需要的数据集

ROS获取KinectV2相机的彩色图和深度图并制作bundlefusion需要的数据集

背景&#xff1a;　最近在研究BundleFusion&#xff0c;跑通官方数据集后&#xff0c;就想着制作自己的数据集来运行bundlefusion&#xff0e;KinectV2相机可直接获取的图像的分辨率分为三个HD 1920x1080, QHD: 960X540&#xff0c;SD: 512x424.我选择是中间的分辨率qhd. 录制…
阅读更多...
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 也明确表示要…
阅读更多...
最新文章

Copyright @ 2022~2023