好久没有更新,之前一直在使用数据集。最近由于需要,使用CeleX的MP相机进行开发。虽然CeleX官方提供了ROS的驱动和demo,但实话实说代码有些乱,版本相比与windows的落后一些,且一些变量名称等与现阶段的不同。最重要的是,不知道为啥,ros的代码写的逻辑很乱,感觉本来很简单的一个事情,又是启动节点,又是订阅发布消息啥的。所以我重新写了一个。**
转载请注明出处**
不依赖ROS的C++程序
首先写一个不依赖ros的纯C++程序,确保路径啥的可以配置正确。参考我之前的文章:【CeleX5事件相机使用系列】第一个c++程序
ROS读取与发布消息
首先写一个节点,用于启动、配置传感器,并读取数据,之后将有用的数据进行发布,供其他节点获取。
#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include "celex5/celex5.h" // celex的头文件,注意放对路径
int main(int argc, char **argv){
ros::init(argc, argv, "celex_ros");
ros::NodeHandle nh;
// celex init
CeleX5 *p = new CeleX5; // 创建一个CeleX5的指针
if (p == NULL){
ROS_ERROR("Cannot create celex pointer...");
return -1;
}
p->openSensor(CeleX5::CeleX5_MIPI); // 打开USB连接的事件相机
if(!p->isSensorReady()){
ROS_ERROR("Sensor init failed...");
return -1;
}
// 设定模式,选择最简单的事件帧模式。
CeleX5::CeleX5Mode sensor_mode = CeleX5::Event_Off_Pixel_Timestamp_Mode;
p->setSensorFixedMode(sensor_mode);
uint8_t * buf = new uint8_t[CELEX5_PIXELS_NUMBER]; // 存储数据,转化为image。这个Number定义为 1280*800
// ROS发布图像数据
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("event_image", 10);
cv_bridge::CvImagePtr frame;
ros::Rate r(30);
while(ros::ok()){
p->getEventPicBuffer(buf, CeleX5::EventBinaryPic); // 官方的读取数据的API函数
cv::Mat img(800, 1280, CV_8UC1, buf); // 官方提供的将buf赋予图像的方法
cv::imshow("frame", img); // 显示图像
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", img).toImageMsg(); // 转化格式
pub.publish(msg); // 发布数据
ros::spinOnce();
r.sleep();
}
return 0;
}
贴一下 CMakeLists.txt 供参考
cmake_minimum_required(VERSION 3.0.2)
project(celex_ros)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
OpenCV REQUIRED
roscpp
std_msgs
celex5_msgs
cv_bridge
image_transport
)
catkin_package()
include_directories(
include
"/PATH_TO_/celex5_msgs/include" # celex5_msgs头文件的路径,如果用到Event.h/EventVector.h等
${catkin_INCLUDE_DIRS}
)
add_executable(celex_ros_node src/main.cpp)
target_link_libraries(celex_ros_node ${catkin_LIBRARIES} ${OpenCV_LIBS} CeleX)
ROS接受消息
接收消息部分相对简单,没有涉及到事件相机的相关操作
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/opencv.hpp>
void imageCallback(const sensor_msgs::ImageConstPtr &msg){
// 图像回调函数,用于显示图像
try{
cv::Mat img = cv_bridge::toCvShare(msg, "mono8")->image;
imshow("img", img);
cv::waitKey(1);
}catch(cv_bridge::Exception &e){
ROS_ERROR("image callback error");
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "recever_test");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("event_image", 10, imageCallback); // 采用 回调方式接收图片
ros::Rate r(30);
while(ros::ok()){
ros::spinOnce();
r.sleep();
}
return 0;
}
编译与运行
使用catkin build
或catkin_make
进行编译,注意编译时路径文件配置正确。编译时需要 celex5_msg 这个消息文件,或许需要编译两次才能完成。
运行时需要将 libCeleX.so 文件(需要自己编译),并将配置文件(cfg_mp 与 cfg_mp_wire)放到工作路径下的"devel/.private/celex_ros/lib/celex_ros"当中,而不是芯仑给的说明书中说的"devel/lib"这个路径(这个路径好像只是个link到了前面的那个路径)。关于库文件编译方法,以及这两个配置文件,还是请看【CeleX5事件相机使用系列】第一个c++程序。
运行时在两个窗口分别运行节点即可:
rosrun celex_ros celex_ros_node # 驱动节点,从传感器读取并发布消息
# in a new terminal
rosrun receiver_test receiver_test_node # 订阅与显示节点,显示驱动读到的图像。
其它
友情链接:kehan大佬写的CeleX_MP在ROS下的驱动 https://github.com/kehanXue/CeleX5-ROS,功能强大,远超官方的驱动。但由于是个人维护,难免会遇到一些配置问题,但大佬还是会帮忙解决的,非常感谢大佬!
微信公众号:【事件相机】,交流事件相机的相关科研与应用。欢迎大家关注