宁波公司核名网站,wordpress输出tags,小网站模板下载地址,技术支持 重庆网站#xff11;#xff0e;目的#xff1a;为了可以标定普通USB相机和固定在相机上的外置IMU的外参,我希望通过ROS获取更高分辨率和更高频率的图像数据,并且可以将图像和imu的topic发布出来,直接使用rosbag record录制话题数据,写入bag文件,这样获得的bag文件直接可以用于相机和…目的为了可以标定普通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_mediumdistribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecasedepth_1-utm_sourcedistribute.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.advertisestd_msgs::String(read, 1000);ros::Publisher IMU_read_pub nh.advertisesensor_msgs::Imu(imu_data, 1000);// ros::Publisher Image_read_pub
// nh.advertisesensor_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::coutacc: std::setprecision(16)imu_data.header.stamp imu_data.linear_acceleration.x imu_data.linear_acceleration.y imu_data.linear_acceleration.zstd::endlstd::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::coutgyr: 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文件
launchnode pkgserialPort typeserialPort nameserialPort requiredtrue outputscreenparam nameport value/dev/ttyUSB0//nodenode pkgimgReader typeimgReader nameimgReader requiredtrue outputscreenparam nameport value/dev/video0//node!-- 在rviz中显示--
node namerviz pkgrviz typerviz args-d $(find imgReader)/config/rviz/get_imu_image.rviz requiredtrue //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_mediumdistribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecasedepth_1-utm_sourcedistribute.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视图窗口了.
就像下图这样.
