├── celex5_msgs ├── msg │ ├── ImuVector.msg │ ├── Event.msg │ ├── EventVector.msg │ └── Imu.msg ├── CMakeLists.txt └── package.xml ├── celex5_calibration ├── src │ ├── intrinsics_extrinsics │ │ ├── README.md │ │ ├── README_zh.md │ │ ├── pkg_camera_calibration │ │ │ ├── assets │ │ │ │ ├── left-0006.png │ │ │ │ ├── left-0013.png │ │ │ │ ├── left-0021.png │ │ │ │ ├── Screenshot from 2020-02-04 23-11-53.png │ │ │ │ └── Screenshot from 2020-02-04 23-28-25.png │ │ │ ├── README_zh.md │ │ │ └── README.md │ │ └── kalibr │ │ │ ├── example_result │ │ │ ├── report-cam-2020-02-12-18-26-45.pdf │ │ │ ├── camchain-2020-02-12-18-26-45.yaml │ │ │ └── results-cam-2020-02-12-18-26-45.txt │ │ │ ├── cfg │ │ │ └── checkerboard.yaml │ │ │ ├── store_sync_images_node.cpp │ │ │ ├── pub_stored_sync_images_node.cpp │ │ │ ├── pub_stored_sync_images.h │ │ │ ├── sync_images.h │ │ │ └── sync_images.cpp │ ├── registration │ │ ├── README.md │ │ ├── event_registration_node.cpp │ │ └── event_registration.h │ ├── pattern │ │ ├── assets │ │ │ ├── Screenshot from 2020-02-04 22-09-31.png │ │ │ ├── Screenshot from 2020-02-04 22-11-21.png │ │ │ ├── Screenshot from 2020-02-04 22-34-03.png │ │ │ └── Screenshot from 2020-02-04 23-11-53.png │ │ ├── pattern_generator_node.cpp │ │ ├── CMakeLists.txt │ │ ├── README_zh.md │ │ ├── pattern_generator.h │ │ └── README.md │ └── temporal │ │ ├── temporal_offset_calibration_node.cpp │ │ ├── temporal_offset_calculate.h │ │ └── temporal_offset_calculate.cpp ├── assets │ └── Screenshot from 2020-02-12 21-02-32.png ├── launch │ ├── store_sync_images_zed_demo.launch │ ├── temporal_offset_calibration_zed_demo.launch │ ├── publish_stored_sync_images_zed_demo.launch │ └── event_registration.launch ├── README_zh.md ├── .gitignore ├── README.md └── package.xml ├── assets ├── 242069421.jpg ├── Screenshot from 2020-01-22 21-16-52.png └── Screenshot from 2020-02-12 21-02-32.png ├── celex5_ros ├── scripts │ ├── celex.rules │ ├── delete_udev_rules.sh │ └── create_udev_rules.sh ├── assets │ ├── Screenshot from 2020-01-22 21-16-52.png │ ├── Screenshot from 2020-01-22 21-20-36.png │ ├── Screenshot from 2020-01-22 21-22-20.png │ └── Screenshot from 2020-01-31 23-57-10.png ├── config │ ├── celex5_frame_parameters.yaml │ └── celex5_events_parameters.yaml ├── src │ ├── sdk │ │ ├── base │ │ │ ├── filedirectory.h │ │ │ ├── dataqueue.h │ │ │ ├── xthread.h │ │ │ ├── filedirectory.cpp │ │ │ ├── xthread.cpp │ │ │ └── dataqueue.cpp │ │ ├── eventproc │ │ │ ├── celex5processeddata.cpp │ │ │ ├── datarecorder.h │ │ │ ├── dataprocessthread.h │ │ │ ├── dataprocessthread.cpp │ │ │ └── datarecorder.cpp │ │ ├── cx3driver │ │ │ ├── cypress.h │ │ │ ├── bulktransfer.h │ │ │ ├── celedriver.h │ │ │ ├── package.h │ │ │ ├── usbinterface.h │ │ │ ├── package.cpp │ │ │ └── cypress.cpp │ │ ├── configproc │ │ │ ├── celex5cfgmgr.h │ │ │ ├── xmlreader.h │ │ │ ├── tinyxml │ │ │ │ ├── tinyxmlerror.cpp │ │ │ │ └── tinystr.cpp │ │ │ ├── wireincommand.h │ │ │ ├── wireincommand.cpp │ │ │ └── celex5cfgmgr.cpp │ │ ├── include │ │ │ ├── celex5 │ │ │ │ ├── celex5processeddata.h │ │ │ │ └── celex5datamanager.h │ │ │ └── celextypes.h │ │ └── CMakeLists.txt │ ├── node │ │ └── celex5_ros_node.cpp │ ├── nodelet │ │ ├── celex5_nodelet.cpp │ │ └── celex5_nodelet.h │ ├── bean │ │ ├── celex5_ros_bean.h │ │ ├── camera_publisher.h │ │ ├── camera_publisher.cpp │ │ ├── celex5_data_forwarder.h │ │ ├── celex5_ros_bean.cpp │ │ └── celex5_options.h │ └── interactive │ │ ├── celex5_display_controller.h │ │ ├── celex5_display_pub_factory.h │ │ └── celex5_ddy_configure.h ├── launch │ ├── celex5_nodelet.launch │ ├── celex5_ros_node.launch │ └── includes │ │ └── nodelet.launch.xml ├── nodelet_plugins.xml ├── .gitignore └── package.xml ├── .gitmodules └── .gitignore /celex5_msgs/msg/ImuVector.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 vector_length 3 | Imu[] imus 4 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/README.md: -------------------------------------------------------------------------------- 1 | *[中文版本](README_zh.md)* 2 | 3 | # TODO -------------------------------------------------------------------------------- /assets/242069421.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/assets/242069421.jpg -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/README_zh.md: -------------------------------------------------------------------------------- 1 | *[English Version](README.md)* 2 | 3 | # TODO -------------------------------------------------------------------------------- /celex5_ros/scripts/celex.rules: -------------------------------------------------------------------------------- 1 | SUBSYSTEMS=="usb", ATTRS{idVendor}=="04b4", ATTRS{idProduct}=="00f1", MODE:="0666" 2 | -------------------------------------------------------------------------------- /assets/Screenshot from 2020-01-22 21-16-52.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/assets/Screenshot from 2020-01-22 21-16-52.png -------------------------------------------------------------------------------- /assets/Screenshot from 2020-02-12 21-02-32.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/assets/Screenshot from 2020-02-12 21-02-32.png -------------------------------------------------------------------------------- /celex5_calibration/src/registration/README.md: -------------------------------------------------------------------------------- 1 | # Registration between Event Camera and RGB Camera 2 | 3 | This part is not perfect, please do not use it. -------------------------------------------------------------------------------- /celex5_ros/assets/Screenshot from 2020-01-22 21-16-52.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_ros/assets/Screenshot from 2020-01-22 21-16-52.png -------------------------------------------------------------------------------- /celex5_ros/assets/Screenshot from 2020-01-22 21-20-36.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_ros/assets/Screenshot from 2020-01-22 21-20-36.png -------------------------------------------------------------------------------- /celex5_ros/assets/Screenshot from 2020-01-22 21-22-20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_ros/assets/Screenshot from 2020-01-22 21-22-20.png -------------------------------------------------------------------------------- /celex5_ros/assets/Screenshot from 2020-01-31 23-57-10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_ros/assets/Screenshot from 2020-01-31 23-57-10.png -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "celex5_calibration/third_party/matplotlib-cpp"] 2 | path = celex5_calibration/third_party/matplotlib-cpp 3 | url = https://github.com/lava/matplotlib-cpp.git 4 | -------------------------------------------------------------------------------- /celex5_calibration/assets/Screenshot from 2020-02-12 21-02-32.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/assets/Screenshot from 2020-02-12 21-02-32.png -------------------------------------------------------------------------------- /celex5_msgs/msg/Event.msg: -------------------------------------------------------------------------------- 1 | # A CeleX5 event 2 | Header header 3 | uint16 x 4 | uint16 y 5 | uint16 brightness 6 | uint64 off_pixel_timestamp 7 | uint64 in_pixel_timestamp 8 | bool polarity 9 | 10 | -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 22-09-31.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 22-09-31.png -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 22-11-21.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 22-11-21.png -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 22-34-03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 22-34-03.png -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 23-11-53.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/pattern/assets/Screenshot from 2020-02-04 23-11-53.png -------------------------------------------------------------------------------- /celex5_msgs/msg/EventVector.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 vector_length 3 | uint32 height # image height, that is, number of rows 4 | uint32 width # image width, that is, number of columns 5 | Event[] events 6 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/left-0006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/left-0006.png -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/left-0013.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/left-0013.png -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/left-0021.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/left-0021.png -------------------------------------------------------------------------------- /celex5_msgs/msg/Imu.msg: -------------------------------------------------------------------------------- 1 | # A CeleX5 imu data 2 | Header header 3 | float64 gyro_x 4 | float64 gyro_y 5 | float64 gyro_z 6 | float64 acc_x 7 | float64 acc_y 8 | float64 acc_z 9 | float64 mag_x 10 | float64 mag_y 11 | float64 mag_z 12 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/example_result/report-cam-2020-02-12-18-26-45.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/intrinsics_extrinsics/kalibr/example_result/report-cam-2020-02-12-18-26-45.pdf -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/Screenshot from 2020-02-04 23-11-53.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/Screenshot from 2020-02-04 23-11-53.png -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/Screenshot from 2020-02-04 23-28-25.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kehanXue/CeleX5-ROS/HEAD/celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/assets/Screenshot from 2020-02-04 23-28-25.png -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/cfg/checkerboard.yaml: -------------------------------------------------------------------------------- 1 | target_type: 'checkerboard' #gridtype 2 | targetCols: 8 #number of internal chessboard corners 3 | targetRows: 6 #number of internal chessboard corners 4 | rowSpacingMeters: 0.0211 #size of one chessboard square [m] 5 | colSpacingMeters: 0.0211 #size of one chessboard square [m] 6 | -------------------------------------------------------------------------------- /celex5_ros/config/celex5_frame_parameters.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 800 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1692.632373, 0.000000, 664.459744, 0.000000, 1695.390478, 379.480451, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.161282, 0.093215, -0.001904, 0.000379, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1655.018311, 0.000000, 665.797738, 0.000000, 0.000000, 1679.957397, 378.563818, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /celex5_ros/config/celex5_events_parameters.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 800 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1687.514919, 0.000000, 676.131784, 0.000000, 1694.799737, 417.348889, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.089225, -0.433622, 0.003022, -0.001306, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1647.456299, 0.000000, 677.701102, 0.000000, 0.000000, 1683.463867, 418.507924, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /celex5_calibration/launch/store_sync_images_zed_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /celex5_calibration/launch/temporal_offset_calibration_zed_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/base/filedirectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef FILEDIRECTORY_H 18 | #define FILEDIRECTORY_H 19 | 20 | #include 21 | 22 | class FileDirectory 23 | { 24 | public: 25 | FileDirectory(); 26 | ~FileDirectory(); 27 | 28 | std::string getApplicationDirPath(); 29 | bool isFileExists(std::string filePath); 30 | }; 31 | 32 | #endif // FILEDIRECTORY_H 33 | -------------------------------------------------------------------------------- /celex5_ros/scripts/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 4 | # 5 | # Copyright (C) 2020 Kehan.Xue 6 | # 7 | # This program is free software: you can redistribute it and/or modify 8 | # it under the terms of the GNU General Public License as published by 9 | # the Free Software Foundation, either version 3 of the License, or 10 | # (at your option) any later version. 11 | # 12 | # This program is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | # GNU General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License 18 | # along with this program. If not, see . 19 | 20 | echo "sudo rm /etc/udev/rules.d/celex.rules" 21 | sudo rm /etc/udev/rules.d/celex.rules 22 | echo "" 23 | echo "Restarting udev" 24 | echo "" 25 | sudo service udev reload 26 | sudo service udev restart 27 | echo "finish delete" -------------------------------------------------------------------------------- /celex5_ros/src/sdk/eventproc/celex5processeddata.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "../include/celex5/celex5processeddata.h" 18 | 19 | CeleX5ProcessedData::CeleX5ProcessedData() 20 | : m_emSensorMode(CeleX5::Event_Off_Pixel_Timestamp_Mode) 21 | , m_iLoopNum(-1) 22 | , m_uiTemperature(0) 23 | , m_uiFullFrameFPS(0) 24 | , m_iFPNProgressValue(0) 25 | { 26 | 27 | } 28 | 29 | CeleX5ProcessedData::~CeleX5ProcessedData() 30 | { 31 | } 32 | -------------------------------------------------------------------------------- /celex5_ros/launch/celex5_nodelet.launch: -------------------------------------------------------------------------------- 1 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /celex5_ros/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 19 | 20 | 21 | 22 | 23 | This is CeleX5-MIPI nodelet. 24 | 25 | 26 | -------------------------------------------------------------------------------- /celex5_ros/scripts/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 4 | # 5 | # Copyright (C) 2020 Kehan.Xue 6 | # 7 | # This program is free software: you can redistribute it and/or modify 8 | # it under the terms of the GNU General Public License as published by 9 | # the Free Software Foundation, either version 3 of the License, or 10 | # (at your option) any later version. 11 | # 12 | # This program is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | # GNU General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License 18 | # along with this program. If not, see . 19 | 20 | 21 | echo "start copy ./celex.rules to /etc/udev/rules.d/" 22 | echo "sudo cp ./celex.rules /etc/udev/rules.d" 23 | sudo cp ./celex.rules /etc/udev/rules.d 24 | echo "" 25 | echo "Restarting udev" 26 | echo "" 27 | sudo service udev reload 28 | sudo service udev restart 29 | echo "finish " -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/example_result/camchain-2020-02-12-18-26-45.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_overlaps: [1] 3 | camera_model: pinhole 4 | distortion_coeffs: [0.012438996402240377, -0.008230936628112907, 0.0011725811652034135, 5 | -0.0025218463105980094] 6 | distortion_model: radtan 7 | intrinsics: [1437.8958223832353, 1437.3087636208852, 1181.7885268260493, 675.0761478882741] 8 | resolution: [2208, 1242] 9 | rostopic: /frame/raw_image 10 | cam1: 11 | T_cn_cnm1: 12 | - [0.9998723854537899, -0.0077667289193917135, 0.013960326960080705, -0.06014964999015901] 13 | - [0.007148637873527468, 0.9990150840929704, 0.043792222269031116, 0.04751368531016086] 14 | - [-0.01428669953113176, -0.04368683642242523, 0.9989431167688675, 0.012981551551057196] 15 | - [0.0, 0.0, 0.0, 1.0] 16 | cam_overlaps: [0] 17 | camera_model: pinhole 18 | distortion_coeffs: [-0.183142928493062, 0.11613943981794345, 0.0026888739143820445, 19 | -0.0009885339013643046] 20 | distortion_model: radtan 21 | intrinsics: [1707.9980790098505, 1705.2747370661336, 631.895926240894, 413.56095397845604] 22 | resolution: [1280, 800] 23 | rostopic: /events/raw_image 24 | -------------------------------------------------------------------------------- /celex5_calibration/launch/publish_stored_sync_images_zed_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/pattern_generator_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include 22 | 23 | #include "pattern_generator.h" 24 | 25 | int main(int argc, char **argv) { 26 | ros::init(argc, argv, "pattern_gen_node"); 27 | ros::NodeHandle nh("~"); 28 | 29 | PatternGenerator pattern_generator; 30 | ros::spin(); 31 | } 32 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/store_sync_images_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include 22 | #include "sync_images.h" 23 | 24 | int main(int argc, char **argv) { 25 | ros::init(argc, argv, "extrinsics_calibration_node"); 26 | ros::NodeHandle nh("~"); 27 | SyncImages sync_images(nh); 28 | ros::spin(); 29 | } 30 | -------------------------------------------------------------------------------- /celex5_ros/src/node/celex5_ros_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #include 21 | 22 | #include "bean/celex5_ros_bean.h" 23 | 24 | using namespace celex5_ros; 25 | 26 | int main(int argc, char **argv) { 27 | 28 | ros::init(argc, argv, "celex5_ros_node"); 29 | ros::NodeHandle nh("~"); 30 | 31 | CeleX5ROSBean celex5_ros_bean(nh); 32 | celex5_ros_bean.Run(); 33 | 34 | ros::spin(); 35 | return 0; 36 | } 37 | 38 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/pub_stored_sync_images_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include 22 | #include "pub_stored_sync_images.h" 23 | 24 | int main(int argc, char **argv) { 25 | ros::init(argc, argv, "pub_stored_sync_images_node"); 26 | ros::NodeHandle nh("~"); 27 | PubStoredSyncImages pub_stored_sync_images(nh); 28 | ros::spin(); 29 | } 30 | -------------------------------------------------------------------------------- /celex5_calibration/src/temporal/temporal_offset_calibration_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include 22 | #include "temporal_offset_calculate.h" 23 | 24 | int main(int argc, char **argv) { 25 | ros::init(argc, argv, "temporal_offset_calibration_node"); 26 | ros::NodeHandle nh("~"); 27 | TemporalOffsetCalculate temporal_offset_calculate(nh); 28 | ros::spin(); 29 | } 30 | -------------------------------------------------------------------------------- /celex5_calibration/launch/event_registration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # BUILD PRODUCTS 2 | *.d 3 | *.o 4 | *.disasm 5 | *.map 6 | *.elf 7 | *.bin 8 | *.hex 9 | *.slo 10 | *.lo 11 | *.obj 12 | 13 | # CMAKE TEMPORARY FILES 14 | CMakeCache.txt 15 | CMakeFiles/ 16 | CMakeFiles/* 17 | 18 | # BUILD FOLDERS 19 | bin/ 20 | bin/* 21 | build/ 22 | build/* 23 | cmake-build-debug/* 24 | 25 | # ROS GENERATED PRODUCTS 26 | msg_gen/ 27 | msg_gen/* 28 | srv_gen/ 29 | srv_gen/* 30 | 31 | # ROS GENERATED PYTHON 32 | _*.py 33 | _*.pyc 34 | 35 | # ROS BAG FILES 36 | *.bag 37 | 38 | # XCODE BUILD PRODUCTS 39 | *.build/ 40 | *.build/* 41 | 42 | # XCODE USER SETTINGS 43 | *.xcuserdatad/ 44 | *.xcuserdatad/* 45 | *.pbxuser 46 | *.pbxuser/* 47 | 48 | # QTCREATOR PROJECT 49 | *.includes 50 | *.creator 51 | *.creator.user 52 | *.config 53 | *.files 54 | 55 | # GEANY PROJECTS 56 | *.geany 57 | 58 | # JETBRAINS 59 | *.idea 60 | 61 | # ZEND/ECLIPSE 62 | *.buildpath 63 | *.settings 64 | *.project 65 | *.pydevproject 66 | 67 | # MACOSX SETTINGS 68 | *.DS_Store 69 | 70 | # GENERATED DOCUMENTATION 71 | html_docs/ 72 | html_docs/* 73 | 74 | # ANDROID SDK GENERATED JAVA 75 | gen/ 76 | gen/* 77 | 78 | # COMPILED DYNAMIC LIBRARIES 79 | *.so 80 | !src/sdk/lib/libCeleX.so 81 | *.dylib 82 | *.dll 83 | 84 | # COMPILED STATIC LIBRARIES 85 | *.lai 86 | *.la 87 | *.a 88 | *.lib 89 | 90 | # EXECUTABLES 91 | *.exe 92 | *.out 93 | *.app 94 | 95 | # Protobuf 96 | *.pb.cc 97 | *.pb.h -------------------------------------------------------------------------------- /celex5_ros/.gitignore: -------------------------------------------------------------------------------- 1 | # BUILD PRODUCTS 2 | *.d 3 | *.o 4 | *.disasm 5 | *.map 6 | *.elf 7 | *.bin 8 | *.hex 9 | *.slo 10 | *.lo 11 | *.obj 12 | 13 | # CMAKE TEMPORARY FILES 14 | CMakeCache.txt 15 | CMakeFiles/ 16 | CMakeFiles/* 17 | 18 | # BUILD FOLDERS 19 | bin/ 20 | bin/* 21 | build/ 22 | build/* 23 | cmake-build-debug/* 24 | 25 | # ROS GENERATED PRODUCTS 26 | msg_gen/ 27 | msg_gen/* 28 | srv_gen/ 29 | srv_gen/* 30 | 31 | # ROS GENERATED PYTHON 32 | _*.py 33 | _*.pyc 34 | 35 | # ROS BAG FILES 36 | *.bag 37 | 38 | # XCODE BUILD PRODUCTS 39 | *.build/ 40 | *.build/* 41 | 42 | # XCODE USER SETTINGS 43 | *.xcuserdatad/ 44 | *.xcuserdatad/* 45 | *.pbxuser 46 | *.pbxuser/* 47 | 48 | # QTCREATOR PROJECT 49 | *.includes 50 | *.creator 51 | *.creator.user 52 | *.config 53 | *.files 54 | 55 | # GEANY PROJECTS 56 | *.geany 57 | 58 | # JETBRAINS 59 | *.idea 60 | 61 | # ZEND/ECLIPSE 62 | *.buildpath 63 | *.settings 64 | *.project 65 | *.pydevproject 66 | 67 | # MACOSX SETTINGS 68 | *.DS_Store 69 | 70 | # GENERATED DOCUMENTATION 71 | html_docs/ 72 | html_docs/* 73 | 74 | # ANDROID SDK GENERATED JAVA 75 | gen/ 76 | gen/* 77 | 78 | # COMPILED DYNAMIC LIBRARIES 79 | *.so 80 | !src/sdk/lib/libCeleX.so 81 | *.dylib 82 | *.dll 83 | 84 | # COMPILED STATIC LIBRARIES 85 | *.lai 86 | *.la 87 | *.a 88 | *.lib 89 | 90 | # EXECUTABLES 91 | *.exe 92 | *.out 93 | *.app 94 | 95 | # Protobuf 96 | *.pb.cc 97 | *.pb.h -------------------------------------------------------------------------------- /celex5_ros/src/nodelet/celex5_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #include "celex5_nodelet.h" 21 | 22 | using namespace celex5_ros; 23 | 24 | CeleX5Nodelet::CeleX5Nodelet() = default; 25 | 26 | CeleX5Nodelet::~CeleX5Nodelet() = default; 27 | 28 | void CeleX5Nodelet::onInit() { 29 | p_celex5_ros_bean_ = std::make_shared(this->getPrivateNodeHandle()); 30 | std::thread thread([&]() { 31 | p_celex5_ros_bean_->Run(); 32 | }); 33 | thread.detach(); 34 | } 35 | 36 | // PLUGINLIB_EXPORT_CLASS(celex5_ros::CeleX5Nodelet, nodelet::Nodelet) 37 | -------------------------------------------------------------------------------- /celex5_calibration/README_zh.md: -------------------------------------------------------------------------------- 1 | *[English Version](README.md)* 2 | 3 | # celex5_calibration 4 | 5 | > The ROS package used for calibrating CeleX™ CeleX5-MIPI Dynamic Vision Sensor. 6 | 7 | 8 | 9 | 本仓库提供了有关于`celex5_calibration`基于Events数据进行标定相关的工具和教程。包括： 10 | 11 | - 标定板生成（目前仅支持闪烁的棋盘格，其他类型待加入） 12 | - 内参标定 13 | - 与其他相机进行外参标定（使用Kalibr） 14 | - 同步收集与定制发布标定所需的图像数据 15 | - 与传统相机进行时间戳对齐（TODO，目前存在问题）等工具 16 | 17 | ## 使用`celex5_calibration` 18 | 19 | 提供了一系列基于Events数据进行相机参数标定的办法。 20 | 21 | - 标定工具的生成 22 | 23 | 运行： 24 | 25 | ```bash 26 | rosrun celex5_ros pattern_generator_node 27 | # In a new Terminal 28 | rosrun rqt_reconfigure rqt_reconfigure # Open rqt_reconfigure to config 29 | ``` 30 | 31 | 更多细节：[链接](src/pattern) 32 | 33 | - 基于ROS中的[camera_calibration](http://wiki.ros.org/camera_calibration/Tutorials)工具包进行的内参标定。（如果要标定与其他相机的外参，ROS中的这个工具仅支持分辨率一样的...） 34 | 35 | 安装： 36 | 37 | ```bash 38 | sudo apt install ros-$ROS_DISTRO-camera-calibration 39 | ``` 40 | 41 | CeleX5-MIPI进行基于该方法进行标定的具体过程：[链接](intrinsics_extrinsics/pkg_camera_calibration) 42 | 43 | - 基于[Kalibr](https://github.com/ethz-asl/kalibr)的标定，支持同时进行内参、与传统相机的外参标定。基于Event数据。 44 | 45 | 更多细节：[链接](src/intrinsics_extrinsics/kalibr) 46 | 47 | 同时提供了收集标定数据的一系列工具。 48 | 49 | - 与其他相机进行时间戳的标定。 50 | 51 | *TODO*，目前仍存在问题。[链接](src/temporal) 52 | 53 | ## 仍存在问题 54 | 55 | **celex5_calibration** 56 | 57 | - 关于与其他相机进行时间戳标定的相关功能还未完成。 -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/example_result/results-cam-2020-02-12-18-26-45.txt: -------------------------------------------------------------------------------- 1 | Calibration results 2 | ==================== 3 | Camera-system parameters: 4 | cam0 (/frame/raw_image): 5 | type: 6 | distortion: [ 0.012439 -0.00823094 0.00117258 -0.00252185] +- [ 0.01077275 0.04834577 0.00082696 0.00064423] 7 | projection: [ 1437.89582238 1437.30876362 1181.78852683 675.07614789] +- [ 1.03115059 0.92398811 0.67579547 2.58675142] 8 | reprojection error: [-0.000031, 0.000075] +- [0.245780, 0.262318] 9 | 10 | cam1 (/events/raw_image): 11 | type: 12 | distortion: [-0.18314293 0.11613944 0.00268887 -0.00098853] +- [ 0.01345115 0.08946159 0.00081123 0.00057281] 13 | projection: [ 1707.99807901 1705.27473707 631.89592624 413.56095398] +- [ 0.91874514 0.95629745 0.33481561 2.07229539] 14 | reprojection error: [0.000059, -0.000018] +- [0.683030, 0.681664] 15 | 16 | baseline T_1_0: 17 | q: [ 0.0218757 -0.00706367 -0.00372985 0.99972879] +- [ 0.00298029 0.00068536 0.00018181] 18 | t: [-0.06014965 0.04751369 0.01298155] +- [ 0.00012073 0.00013789 0.000869 ] 19 | 20 | 21 | 22 | Target configuration 23 | ==================== 24 | 25 | Type: checkerboard 26 | Rows 27 | Count: 6 28 | Distance: 0.0211 [m] 29 | Cols 30 | Count: 8 31 | Distance: 0.0211 [m] 32 | -------------------------------------------------------------------------------- /celex5_calibration/.gitignore: -------------------------------------------------------------------------------- 1 | # BUILD PRODUCTS 2 | *.d 3 | *.o 4 | *.disasm 5 | *.map 6 | *.elf 7 | *.bin 8 | *.hex 9 | *.slo 10 | *.lo 11 | *.obj 12 | 13 | # CMAKE TEMPORARY FILES 14 | CMakeCache.txt 15 | CMakeFiles/ 16 | CMakeFiles/* 17 | 18 | # BUILD FOLDERS 19 | bin/ 20 | bin/* 21 | build/ 22 | build/* 23 | cmake-build-debug/* 24 | 25 | # ROS GENERATED PRODUCTS 26 | msg_gen/ 27 | msg_gen/* 28 | srv_gen/ 29 | srv_gen/* 30 | 31 | # ROS GENERATED PYTHON 32 | _*.py 33 | _*.pyc 34 | 35 | # ROS BAG FILES 36 | *.bag 37 | 38 | # XCODE BUILD PRODUCTS 39 | *.build/ 40 | *.build/* 41 | 42 | # XCODE USER SETTINGS 43 | *.xcuserdatad/ 44 | *.xcuserdatad/* 45 | *.pbxuser 46 | *.pbxuser/* 47 | 48 | # QTCREATOR PROJECT 49 | *.includes 50 | *.creator 51 | *.creator.user 52 | *.config 53 | *.files 54 | 55 | # GEANY PROJECTS 56 | *.geany 57 | 58 | # JETBRAINS 59 | *.idea 60 | 61 | # ZEND/ECLIPSE 62 | *.buildpath 63 | *.settings 64 | *.project 65 | *.pydevproject 66 | 67 | # MACOSX SETTINGS 68 | *.DS_Store 69 | 70 | # GENERATED DOCUMENTATION 71 | html_docs/ 72 | html_docs/* 73 | 74 | # ANDROID SDK GENERATED JAVA 75 | gen/ 76 | gen/* 77 | 78 | # COMPILED DYNAMIC LIBRARIES 79 | *.so 80 | !src/sdk/lib/libCeleX.so 81 | *.dylib 82 | *.dll 83 | 84 | # COMPILED STATIC LIBRARIES 85 | *.lai 86 | *.la 87 | *.a 88 | *.lib 89 | 90 | # EXECUTABLES 91 | *.exe 92 | *.out 93 | *.app 94 | 95 | # Protobuf 96 | *.pb.cc 97 | *.pb.h 98 | 99 | output/ 100 | output/* -------------------------------------------------------------------------------- /celex5_calibration/src/registration/event_registration_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include 22 | #include "event_registration.h" 23 | 24 | int main(int argc, char **argv) { 25 | ros::init(argc, argv, "event_registration_node"); 26 | ros::NodeHandle nh("~"); 27 | 28 | // TicToc t_start; 29 | // ros::Time start = ros::Time::now(); 30 | // for (int i = 0; i < 2280 * 1242; ++i) { 31 | // double b = 1.2 * 3; 32 | // } 33 | // std::cout << (ros::Time::now() - start).toSec() << std::endl; 34 | // std::cout << t_start.toc() << std::endl; 35 | EventRegistration event_registration(nh); 36 | ros::spin(); 37 | } 38 | -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ## 2 | # The celex5_calibration ROS package, 3 | # used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | # 5 | # Copyright (C) 2020 Kehan.Xue 6 | # 7 | # This program is free software: you can redistribute it and/or modify 8 | # it under the terms of the GNU General Public License as published by 9 | # the Free Software Foundation, either version 3 of the License, or 10 | # (at your option) any later version. 11 | # 12 | # This program is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | # GNU General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License 18 | # along with this program. If not, see . 19 | 20 | cmake_minimum_required(VERSION 2.8.3) 21 | project(pattern_generator) 22 | 23 | add_compile_options(-std=c++14) 24 | find_package(catkin REQUIRED COMPONENTS 25 | roscpp 26 | ddynamic_reconfigure) 27 | 28 | find_package(OpenCV 3 REQUIRED) 29 | include_directories(${OpenCV_INCLUDE_DIRS}) 30 | include_directories(${catkin_INCLUDE_DIRS}) 31 | 32 | add_executable(pattern_generator_node 33 | pattern_generator_node.cpp 34 | pattern_generator.h 35 | pattern_generator.cpp) 36 | 37 | target_link_libraries(pattern_generator_node 38 | ${OpenCV_LIBS} 39 | ${catkin_LIBRARIES}) 40 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/cx3driver/cypress.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CYPRESS_H 18 | #define CYPRESS_H 19 | 20 | #include "usbinterface.h" 21 | 22 | typedef enum 23 | { 24 | CTRL_I2C_SET_REG = 0x100, 25 | CTRL_I2C_GET_REG = 0x200, 26 | CTRL_MIPI_SET_REG = 0x300, 27 | CTRL_MIPI_GET_REG = 0x400, 28 | }CONTROL_COMMAND; 29 | 30 | class Cypress : public USBInterface 31 | { 32 | public: 33 | Cypress(); 34 | virtual ~Cypress(); 35 | 36 | bool openUSB(void); 37 | bool openStream(void); 38 | 39 | void closeUSB(void); 40 | void closeStream(void); 41 | 42 | bool usbSet(uint16_t wId, uint16_t reg, uint16_t value); 43 | bool usbGet(uint16_t wId, uint16_t reg, uint16_t &value); 44 | 45 | bool writeSerialNumber(std::string number); 46 | std::string getSerialNumber(); 47 | std::string getFirmwareVersion(); 48 | std::string getFirmwareDate(); 49 | }; 50 | 51 | #endif // CYPRESS_H 52 | -------------------------------------------------------------------------------- /celex5_ros/src/nodelet/celex5_nodelet.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef CELEX5_ROS_SRC_BEAN_CELEX5_NODELET_H_ 21 | #define CELEX5_ROS_SRC_BEAN_CELEX5_NODELET_H_ 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "bean/celex5_ros_bean.h" 31 | 32 | namespace celex5_ros { 33 | class CeleX5Nodelet : public nodelet::Nodelet { 34 | 35 | public: 36 | CeleX5Nodelet(); 37 | ~CeleX5Nodelet() override; 38 | 39 | private: 40 | void onInit() override; 41 | std::shared_ptr p_celex5_ros_bean_; 42 | }; 43 | } 44 | PLUGINLIB_EXPORT_CLASS(celex5_ros::CeleX5Nodelet, nodelet::Nodelet) 45 | 46 | #endif //CELEX5_ROS_SRC_BEAN_CELEX5_NODELET_H_ 47 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/configproc/celex5cfgmgr.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CELEX5CFGMGR_H 18 | #define CELEX5CFGMGR_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "../include/celex5/celex5.h" 25 | 26 | class WireinCommand; 27 | 28 | /* 29 | * This class is used to process the config file of CeleX5. 30 | * Including parse the config file, save the file and get 31 | * the parameters in the config file. 32 | */ 33 | class CeleX5CfgMgr 34 | { 35 | public: 36 | CeleX5CfgMgr(); 37 | ~CeleX5CfgMgr(); 38 | 39 | bool parseCeleX5Cfg(const std::string& cfgName); 40 | bool saveCeleX5XML(std::map>& mapCfgInfo); 41 | bool saveCeleX5XML(const std::string& cfgName); 42 | std::map> getCeleX5Cfg(); 43 | 44 | private: 45 | std::map m_CommandMapCeleX5; 46 | std::map> m_CommandListCeleX5; 47 | }; 48 | 49 | #endif // CELEX5CFGMGR_H 50 | -------------------------------------------------------------------------------- /celex5_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(celex5_msgs) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED 8 | roscpp 9 | rospy 10 | std_msgs 11 | message_generation) 12 | 13 | 14 | ## Generate messages in the 'msg' folder 15 | add_message_files( 16 | FILES 17 | Event.msg 18 | EventVector.msg 19 | Imu.msg 20 | ImuVector.msg 21 | ) 22 | 23 | ## Generate added messages and services with any dependencies listed here 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs # Or other packages containing msgs 27 | ) 28 | 29 | ################################### 30 | ## catkin specific configuration ## 31 | ################################### 32 | ## The catkin_package macro generates cmake config files for your package 33 | ## Declare things to be passed to dependent projects 34 | ## INCLUDE_DIRS: uncomment this if your package contains header files 35 | ## LIBRARIES: libraries you create in this project that dependent projects also need 36 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 37 | ## DEPENDS: system dependencies of this project that dependent projects also need 38 | catkin_package( 39 | # INCLUDE_DIRS include 40 | # LIBRARIES celex5_msgs 41 | # CATKIN_DEPENDS other_catkin_pkg 42 | CATKIN_DEPENDS message_runtime 43 | # DEPENDS system_lib 44 | ) 45 | 46 | ########### 47 | ## Build ## 48 | ########### 49 | 50 | ## Specify additional locations of header files 51 | ## Your package locations should be listed before other locations 52 | include_directories( 53 | # include 54 | # ${catkin_INCLUDE_DIRS} 55 | ) 56 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/eventproc/datarecorder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef DATARECORDER_H 18 | #define DATARECORDER_H 19 | 20 | #include 21 | #include "../include/celex5/celex5.h" 22 | 23 | class DataRecorder 24 | { 25 | public: 26 | DataRecorder(); 27 | ~DataRecorder(); 28 | 29 | /* 30 | * @function: isRecording 31 | * @brief : check if it is recording 32 | * @input : 33 | * @output : 34 | * @return : true is recording, false is not recording 35 | */ 36 | bool isRecording() 37 | { 38 | return m_bRecording; 39 | } 40 | bool startRecording(std::string filePath); 41 | void stopRecording(CeleX5::BinFileAttributes* header); 42 | bool writeData(uint8_t* pData, uint32_t length); 43 | bool writeData(uint8_t* pData, uint32_t length, std::time_t time_stamp_end, std::vector imuData); 44 | 45 | private: 46 | int getLocalTimestamp(); 47 | 48 | private: 49 | bool m_bRecording; 50 | int m_iTimeStampStart; 51 | std::ofstream m_ofstreamRecord; 52 | uint32_t m_uiPackageCount; 53 | }; 54 | 55 | #endif // DATARECORDER_H 56 | 57 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/cx3driver/bulktransfer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef __BULK_TRANSFER_H__ 18 | #define __BULK_TRANSFER_H__ 19 | 20 | #include 21 | #include "package.h" 22 | #include "../cx3driver/include/libusb.h" 23 | 24 | #define MAX_IMAGE_BUFFER_NUMBER 100 25 | 26 | //#include 27 | //extern std::ofstream ofTest; 28 | 29 | extern bool g_bTransferError; 30 | extern bool g_bUsingIMUCallback; 31 | extern uint16_t g_uiTailIndex; 32 | extern uint16_t g_uiHeadIndex; 33 | extern uint32_t g_uiPackageCount; 34 | extern CPackage g_PackageList[MAX_IMAGE_BUFFER_NUMBER]; 35 | // 36 | extern std::vector g_IMURawDataList; 37 | extern std::mutex g_mtxSensorData; 38 | 39 | libusb_transfer *alloc_bulk_transfer(libusb_device_handle *device_handle, uint8_t address, uint8_t *buffer); 40 | libusb_transfer *alloc_interrupt_transfer(libusb_device_handle *device_handle, uint8_t address, uint8_t *buffer); //added by xiaoqin @2019.06.11 for receiving IMU data 41 | void cancel_bulk_transfer(libusb_transfer *xfr); 42 | 43 | #endif // __BULK_TRANSFER_H__ 44 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/configproc/xmlreader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef XMLREADER_H 18 | #define XMLREADER_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "../include/celex5/celex5.h" 27 | 28 | class TiXmlDocument; 29 | class WireinCommand; 30 | 31 | /* 32 | * This class is used to process the xml file. 33 | * Including parse the xml file, save the file. 34 | */ 35 | class XmlReader 36 | { 37 | public: 38 | XmlReader(); 39 | ~XmlReader(); 40 | 41 | bool parse(const std::string& filename, TiXmlDocument* pDom); 42 | bool save(const std::string& filename, TiXmlDocument* pDom); 43 | bool importCeleX5Commands(std::map >& commandList, TiXmlDocument* pDom); 44 | 45 | bool saveXML(const std::map >& mapCfgInfo); 46 | bool saveXML(const std::string& filename); 47 | 48 | private: 49 | bool getNumber(const std::string& text, uint32_t* pNumber); 50 | CeleX5::CfgInfo getCfgInfoByName(const std::string& csrType, const std::string& name, const std::map >& mapCfg); 51 | }; 52 | 53 | #endif // XMLREADER_H 54 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/cx3driver/celedriver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CELEPIXEL_H 18 | #define CELEPIXEL_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "../include/celextypes.h" 25 | 26 | class Cypress; 27 | class CeleDriver 28 | { 29 | public: 30 | CeleDriver(void); 31 | ~CeleDriver(void); 32 | 33 | public: 34 | bool openUSB(); 35 | bool openStream(); 36 | 37 | void closeUSB(); 38 | void closeStream(); 39 | 40 | bool writeSerialNumber(std::string number); 41 | std::string getSerialNumber(); 42 | std::string getFirmwareVersion(); 43 | std::string getFirmwareDate(); 44 | 45 | bool getSensorData(uint8_t* pData, uint32_t& length); 46 | bool getSensorData(uint8_t* pData, uint32_t& length, std::time_t& timestampEnd, std::vector& imuData); //added by xiaoqin @2019.01.24 47 | void clearData(); 48 | 49 | uint16_t getALSValue(); 50 | 51 | public: 52 | bool i2cSet(uint16_t reg, uint16_t value); 53 | bool i2cGet(uint16_t reg, uint16_t &value); 54 | bool mipiSet(uint16_t reg, uint16_t value); 55 | bool mipiGet(uint16_t reg, uint16_t &value); 56 | 57 | private: 58 | Cypress* m_pCypress; 59 | }; 60 | 61 | #endif // CELEPIXEL_H 62 | -------------------------------------------------------------------------------- /celex5_ros/src/bean/celex5_ros_bean.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef CELEX5_ROS_SRC_BEAN_CELEX5ROSBEAN_H_ 21 | #define CELEX5_ROS_SRC_BEAN_CELEX5ROSBEAN_H_ 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include "celex5/celex5datamanager.h" 29 | #include "celex5_options.h" 30 | #include "interactive/celex5_ddy_configure.h" 31 | #include "interactive/celex5_display_controller.h" 32 | #include "celex5_data_forwarder.h" 33 | 34 | namespace celex5_ros { 35 | class CeleX5ROSBean { 36 | public: 37 | explicit CeleX5ROSBean(const ros::NodeHandle &nh); 38 | virtual ~CeleX5ROSBean(); 39 | void Run(); 40 | 41 | private: 42 | void ReadParams(); 43 | 44 | std::shared_ptr p_celex5_options_; 45 | std::shared_ptr p_celex5_sensor_; 46 | std::shared_ptr p_celex5_configure_; 47 | std::shared_ptr p_celex5_data_forwarder_; 48 | 49 | std::shared_ptr p_celex5_display_; 50 | 51 | ros::NodeHandle nh_; 52 | }; 53 | } 54 | 55 | #endif //CELEX5_ROS_SRC_BEAN_CELEX5ROSBEAN_H_ 56 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/base/dataqueue.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef DATAQUEUE_H 18 | #define DATAQUEUE_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "../include/celextypes.h" 25 | 26 | #define ARRAY_SIZE 12 27 | #define MAX_DATA_LEN 1536001 28 | 29 | typedef struct DataInfo 30 | { 31 | uint8_t* pData; 32 | unsigned long length; 33 | time_t timestamp; 34 | std::vector vecIMUData; 35 | } DataInfo; 36 | 37 | /* 38 | * This class provides a data queue for playing back recorded bin file. 39 | */ 40 | class DataQueue 41 | { 42 | public: 43 | DataQueue(); 44 | ~DataQueue(); 45 | 46 | bool push(uint8_t* pData, uint32_t length, std::time_t timestamp = 0); 47 | bool push(uint8_t* pData, uint32_t length, std::vector &imuData, std::time_t timestamp); 48 | 49 | bool pop(uint8_t* pData, uint32_t* length, std::time_t* timestamp); 50 | bool pop(uint8_t* pData, uint32_t* length, std::vector &imuData, std::time_t* timestamp); 51 | 52 | uint32_t size(); 53 | void clear(); 54 | 55 | private: 56 | std::array m_arrayData; 57 | uint32_t m_uiSize; 58 | int m_iHead; 59 | int m_iTail; 60 | }; 61 | 62 | #endif // DATAQUEUE_H 63 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/cx3driver/package.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CPACKAGE_H 18 | #define CPACKAGE_H 19 | 20 | #include 21 | #include 22 | #include "celedriver.h" 23 | 24 | typedef enum 25 | { 26 | BUFFER_STATUS_EMPTY = 0, 27 | BUFFER_STATUS_FULL, 28 | }BUFFER_STATUS; 29 | 30 | #define MAX_ELEMENT_BUFFER_SIZE 43008 // 42976 / 24576 / 24544 / 32752 31 | #define NOT_USE_VECTOR 32 | 33 | class CElement 34 | { 35 | public: 36 | CElement(); 37 | ~CElement(); 38 | 39 | public: 40 | uint8_t *begin(); 41 | uint8_t *end(); 42 | void save(uint8_t *buffer, uint16_t wLen); 43 | 44 | private: 45 | uint8_t data[MAX_ELEMENT_BUFFER_SIZE]; 46 | uint16_t wdataLen; 47 | }; 48 | 49 | class CPackage 50 | { 51 | public: 52 | CPackage(); 53 | ~CPackage(); 54 | 55 | public: 56 | void insert(uint8_t *buffer, uint16_t wLen); 57 | bool getImage(std::vector &image); 58 | bool getData(uint8_t* pData, uint32_t& length); 59 | void end(); 60 | void clearData(); 61 | int size(); 62 | 63 | std::time_t m_lTimestampEnd; 64 | std::vector m_vecIMUData; 65 | 66 | private: 67 | std::vector m_vecElementList; 68 | BUFFER_STATUS m_emStatus; 69 | size_t m_uiOffset; 70 | // 71 | uint8_t* m_pPackageBuffer; 72 | }; 73 | 74 | #endif // CPACKAGE_H -------------------------------------------------------------------------------- /celex5_ros/src/sdk/configproc/tinyxml/tinyxmlerror.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | www.sourceforge.net/projects/tinyxml 3 | Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com) 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any 7 | damages arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any 10 | purpose, including commercial applications, and to alter it and 11 | redistribute it freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must 14 | not claim that you wrote the original software. If you use this 15 | software in a product, an acknowledgment in the product documentation 16 | would be appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and 19 | must not be misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source 22 | distribution. 23 | */ 24 | 25 | #include "tinyxml.h" 26 | 27 | // The goal of the seperate error file is to make the first 28 | // step towards localization. tinyxml (currently) only supports 29 | // english error messages, but the could now be translated. 30 | // 31 | // It also cleans up the code a bit. 32 | // 33 | 34 | const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] = 35 | { 36 | "No error", 37 | "Error", 38 | "Failed to open file", 39 | "Error parsing Element.", 40 | "Failed to read Element name", 41 | "Error reading Element value.", 42 | "Error reading Attributes.", 43 | "Error: empty tag.", 44 | "Error reading end tag.", 45 | "Error parsing Unknown.", 46 | "Error parsing Comment.", 47 | "Error parsing Declaration.", 48 | "Error document empty.", 49 | "Error null (0) or unexpected EOF found in input stream.", 50 | "Error parsing CDATA.", 51 | "Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.", 52 | }; 53 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/base/xthread.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef XTHREAD_H 18 | #define XTHREAD_H 19 | 20 | #include 21 | 22 | #ifdef _WIN32 23 | #include 24 | #include 25 | #else 26 | #include 27 | #endif 28 | 29 | class XThread 30 | { 31 | public: 32 | XThread(const std::string threadName = "newthread"); 33 | virtual ~XThread(); 34 | 35 | virtual void run() = 0; 36 | virtual bool start(bool bSuspended = false); 37 | 38 | void join(int timeout = -1); 39 | void resume(); 40 | void suspend(); 41 | bool terminate(); 42 | bool isRunning(); 43 | 44 | unsigned int getThreadID(); 45 | std::string getThreadName(); 46 | void setThreadName(std::string threadName); 47 | 48 | private: 49 | bool createThread(bool bSuspended = false); 50 | #ifdef _WIN32 51 | static unsigned int WINAPI staticThreadFunc(void* args); 52 | #else 53 | static void* staticThreadFunc(void* args); 54 | #endif 55 | 56 | protected: 57 | std::string m_threadName; 58 | volatile bool m_bRun; 59 | bool m_bSuspended; 60 | 61 | #ifdef _WIN32 62 | HANDLE m_handle; 63 | unsigned int m_threadID; 64 | #else 65 | pthread_t m_threadID; 66 | pthread_mutex_t m_mutex; 67 | pthread_cond_t m_cond; 68 | #endif 69 | }; 70 | 71 | #endif // XTHREAD_H 72 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/include/celex5/celex5processeddata.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CELEX5_PROCESSED_DATA_H 18 | #define CELEX5_PROCESSED_DATA_H 19 | 20 | #include "celex5.h" 21 | 22 | /* 23 | * This class is used to get the sensor data includes image 24 | * buffer, raw event data and some setting parameters. 25 | */ 26 | class CeleX5ProcessedData 27 | { 28 | public: 29 | CeleX5ProcessedData(); 30 | ~CeleX5ProcessedData(); 31 | inline CeleX5::CeleX5Mode getSensorMode() { return m_emSensorMode; } 32 | inline void setSensorMode(CeleX5::CeleX5Mode mode) { m_emSensorMode = mode; } 33 | inline int getLoopNum() { return m_iLoopNum; } 34 | inline void setLoopNum(int loopNum) { m_iLoopNum = loopNum; } 35 | inline void setTemperature(uint16_t temperature) { m_uiTemperature = temperature; } 36 | inline uint16_t getTemperature() { return m_uiTemperature; } 37 | inline void setFullFrameFPS(uint16_t fps) { m_uiFullFrameFPS = fps; } 38 | inline uint16_t getFullFrameFPS() { return m_uiFullFrameFPS; } 39 | inline void updateFPNProgress(int value) { m_iFPNProgressValue = value; } 40 | inline int getFPNProgress() { return m_iFPNProgressValue; } 41 | 42 | private: 43 | CeleX5::CeleX5Mode m_emSensorMode; 44 | int m_iLoopNum; 45 | uint16_t m_uiTemperature; 46 | uint16_t m_uiFullFrameFPS; 47 | int m_iFPNProgressValue; 48 | }; 49 | 50 | #endif // CELEX5_PROCESSED_DATA_H 51 | -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/README_zh.md: -------------------------------------------------------------------------------- 1 | *[English Version](README.md)* 2 | 3 | # Calibration Pattern Generator 4 | 5 | > ROS node for generating **Blinking-Chessboard** calibration pattern for Event-based Camera(Dynamic Vision Sensor). 6 | 7 | - [概述](#概述) 8 | - [编译与运行](#编译与运行) 9 | - [CeleX5-MIPI的内参标定样例](#celex5-mipi的内参标定样例) 10 | 11 | ## 概述 12 | 13 | 提供了一个ROS节点，用于生成标定相机的闪烁的棋盘格。提供了`rqt_reconfigure`的支持以来控制棋盘格的大小、闪烁的频率。并提供将其保存成.avi视频格式。 14 | 15 | Chessboard 16 | 17 | 18 | 19 | ## 编译与运行 20 | 21 | 1. 编译 22 | 23 | 如果还未编译整个仓库，请克隆整个仓库并进行编译（如果已经进行过，请略过这一步）。 24 | 25 | ```bash 26 | mkdir -p ~/celex_ws/src 27 | cd ~/celex_ws/src 28 | git clone git@github.com:kehanXue/CeleX5-ROS.git 29 | # Or with http: `git clone https://github.com/kehanXue/CeleX5-ROS.git` 30 | cd .. 31 | rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 32 | catkin_make # Or use `catkin build` 33 | ``` 34 | 35 | 2. 运行 36 | 37 | ```bash 38 | source ~/celex_ws/devel/setup.bash # Or source setup.zsh when you use zsh 39 | rosrun celex5_ros pattern_generator_node 40 | # In a new Terminal 41 | rosrun rqt_reconfigure rqt_reconfigure # Open rqt_reconfigure to config 42 | ``` 43 | 44 | 无报错即正常运行。你可以看到会弹出一个窗口显示正在闪烁的棋盘格标定板。此时，你可以通过`rqt_reconfigure`面板来调整标定板的参数： 45 | 46 | - `square_size`：棋盘格中每个格子方块的边长。单位为像素。 47 | 48 | - `row_corners_num`和`col_corners_num`：生成的棋盘格的行和列的内点数目。 49 | 50 | - `blinking_fps`：棋盘格闪烁的频率/帧率。一次闪烁与下次闪烁开始前为一个周期。 51 | 52 | - `static_board`：控制棋盘格是否闪烁。 53 | 54 | - `record_video`：点击开始生成闪烁棋盘格的视频，再次点击（不勾选）停止录制视频。视频中棋盘格闪烁的频率与当前`blinking_fps`参数一致。注意，在生成视频过程中调整`blinking_fps`对生成的视频是无效的，且视频生成过程中不要调整棋盘格的大小！ 55 | 56 | 视频会生成在当前运行的路径下： 57 | 58 | 59 | 60 | 61 | 62 | ## CeleX5-MIPI的内参标定样例 63 | 64 | [CeleX5-MIPI的内参标定样例](../intrinsics) 65 | 66 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/pub_stored_sync_images.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #ifndef CELEX5_CALIBRATION_SRC_EXTRINSICS_PUBSTOREDSYNCIMAGES_H_ 32 | #define CELEX5_CALIBRATION_SRC_EXTRINSICS_PUBSTOREDSYNCIMAGES_H_ 33 | 34 | class PubStoredSyncImages { 35 | public: 36 | explicit PubStoredSyncImages(const ros::NodeHandle &nh = ros::NodeHandle("~"), 37 | bool is_publish_enable = false, 38 | int publish_rate = 30); 39 | virtual ~PubStoredSyncImages(); 40 | private: 41 | ros::NodeHandle nh_; 42 | std::vector image1_filenames_; 43 | std::vector image2_filenames_; 44 | 45 | ros::Publisher image1_pub_; 46 | ros::Publisher image2_pub_; 47 | 48 | std::shared_ptr p_ddyn_rec_; 49 | bool is_publish_enable_; 50 | int publish_rate_; 51 | 52 | std::shared_ptr p_main_thread_; 53 | }; 54 | 55 | #endif //CELEX5_CALIBRATION_SRC_EXTRINSICS_PUBSTOREDSYNCIMAGES_H_ 56 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/configproc/wireincommand.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef WIREINCOMMAND_H 18 | #define WIREINCOMMAND_H 19 | 20 | #include 21 | #include 22 | 23 | /* 24 | * This class is used to set and get the parameters of register . 25 | */ 26 | class WireinCommand 27 | { 28 | public: 29 | enum RegisterType 30 | { 31 | R_High = 0, 32 | R_Middle = 1, 33 | R_Low = 2 34 | }; 35 | WireinCommand(const std::string& name); 36 | 37 | ~WireinCommand(); 38 | 39 | void setValue(int16_t value, RegisterType type); 40 | void combineRegBytes(); 41 | 42 | void setAddress(int16_t address, RegisterType type); 43 | 44 | void setMaxValue(int32_t value); 45 | void setMinValue(int32_t value); 46 | 47 | std::string getName() { return m_strName; } 48 | int32_t getValue() { return m_iValue; } 49 | int16_t getHighAddr() { return m_iAddressH; } 50 | int16_t getMiddleAddr() { return m_iAddressM; } 51 | int16_t getLowAddr() { return m_iAddressL; } 52 | int32_t getMaxValue() { return m_iMaxValue; } 53 | int32_t getMinValue() { return m_iMinValue; } 54 | 55 | private: 56 | //std::map m_mapRegister; //key: address; value: value 57 | std::string m_strName; 58 | int16_t m_iAddressH; 59 | int16_t m_iAddressM; 60 | int16_t m_iAddressL; 61 | int16_t m_iValueH; 62 | int16_t m_iValueM; 63 | int16_t m_iValueL; 64 | int32_t m_iValue; 65 | int32_t m_iMinValue; 66 | int32_t m_iMaxValue; 67 | }; 68 | 69 | #endif // WIREINCOMMAND_H 70 | -------------------------------------------------------------------------------- /celex5_calibration/README.md: -------------------------------------------------------------------------------- 1 | *[中文版本](README_zh.md)* 2 | 3 | # celex5_calibration 4 | 5 | > The ROS package used for calibrating CeleX™ CeleX5-MIPI Dynamic Vision Sensor. 6 | 7 | 8 | 9 | This repository provides tools and tutorials related to the calibration of CeleX5-MIPI, based on events data. Including: 10 | 11 | - calibration board generation (currently only support the blinking checkerboard, other types to be added) 12 | - Intrinsics calibration 13 | - Extrinsics calibration with another traditional frame camera (using [Kalibr](https://github.com/ethz-asl/kalibr)) 14 | - Synchronous data collection and publishing of image data required for calibration 15 | - Time stamp alignment with traditional camera (*TODO*, existing problems) 16 | 17 | ## Run `celex5_calibration`: 18 | 19 | A series of methods and tools for camera parameter calibration based on events data are provided. 20 | 21 | - Pattern generator (blinking chessboard). [More details](src/pattern) 22 | 23 | Run: 24 | 25 | ```bash 26 | rosrun celex5_ros pattern_generator_node 27 | # In a new Terminal 28 | rosrun rqt_reconfigure rqt_reconfigure # Open rqt_reconfigure to config 29 | ``` 30 | 31 | - Intrinsics calibration based on [camera_calibration](http://wiki.ros.org/camera_calibration/Tutorials) toolkit in ROS. (if you want to calibrate the external parameters of other cameras, this tool in ROS only supports the same resolution...) 32 | 33 | Install: 34 | 35 | ```bash 36 | sudo apt install ros-$ROS_DISTRO-camera-calibration 37 | ``` 38 | 39 | The detailed tutorials: [link](src/intrinsics_extrinsics/pkg_camera_calibration) 40 | 41 | - Based on [Kalibr](https://github.com/ethz-asl/kalibr) toolkit, which supports both intrinsics calibration and extrinsics calibration with another (traditional or event-based) cameras, in different resolutions. Event data based method. 42 | 43 | The detailed tutorials: [link](src/intrinsics_extrinsics/kalibr) 44 | 45 | Also provides a series of tools to collect calibration data of CeleX5-MIPI Camera. 46 | 47 | - Time stamp calibration with another camera. 48 | 49 | *TODO*, existing problems. [link](src/temporal) 50 | 51 | ## Known Issues 52 | 53 | **celex5_calibration** 54 | 55 | - The time stamp calibration with another camera. -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/pattern_generator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef CELEX5_ROS_SRC_CALIBRATION_PATTERNGENERATOR_H_ 22 | #define CELEX5_ROS_SRC_CALIBRATION_PATTERNGENERATOR_H_ 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | class PatternGenerator { 32 | public: 33 | explicit PatternGenerator(const ros::NodeHandle &nh = ros::NodeHandle("~"), 34 | int square_size = 100, 35 | int col_corners_num = 8, 36 | int row_corners_num = 6, 37 | bool static_board = false, 38 | int blinking_fps = 15, 39 | bool record_video = false, 40 | bool show_img = true); 41 | virtual ~PatternGenerator(); 42 | 43 | private: 44 | void CreateChessBoard(); 45 | void CreateBlackBoard(); 46 | 47 | int square_size_; 48 | int col_corners_num_; 49 | int row_corners_num_; 50 | 51 | bool static_board_; 52 | int blinking_fps_; 53 | cv::Mat pattern_board_; 54 | 55 | bool record_video_; 56 | bool show_img_; 57 | 58 | ros::NodeHandle nh_; 59 | std::shared_ptr p_ddyn_rec_; 60 | std::shared_ptr p_thread_; 61 | }; 62 | 63 | #endif //CELEX5_ROS_SRC_CALIBRATION_PATTERNGENERATOR_H_ 64 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/cx3driver/usbinterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef USBINTERFACE_H 18 | #define USBINTERFACE_H 19 | 20 | #include 21 | 22 | #ifdef __linux__ 23 | #include 24 | #else 25 | #include 26 | #include 27 | #endif 28 | #include "bulktransfer.h" 29 | 30 | #define MAX_URB_NUMBER 20 31 | #define MAX_URB_NUMBER_IMU 20 32 | 33 | class USBInterface 34 | { 35 | public: 36 | USBInterface(); 37 | ~USBInterface(); 38 | 39 | bool videoStart(void); 40 | void videoStop(void); 41 | 42 | public: 43 | bool usbOpen(int vid, int pid, int transMode); 44 | void usbClose(void); 45 | bool usbControl(uint8_t requestType, uint8_t request, uint16_t value, uint16_t index, uint8_t *buffer, uint16_t len); 46 | 47 | bool start(void); 48 | void stop(void); 49 | 50 | private: 51 | bool usbCheckDevice(libusb_device *dev, int vid, int pid, int transMode); 52 | bool usbGetInterface(int vid, int pid, int transMode); 53 | bool usbAllocBulkTransfer(void); 54 | bool usbAllocInterruptTransfer(void); //added by xiaoqin @2019.06.11 for receiving IMU data 55 | 56 | void worker(void); 57 | 58 | private: 59 | libusb_device_handle* m_pDeviceHandle; 60 | 61 | libusb_transfer* m_pBulkTransfer[MAX_URB_NUMBER]; 62 | uint8_t m_uiBulkBuffer[MAX_URB_NUMBER][MAX_ELEMENT_BUFFER_SIZE]; 63 | 64 | libusb_transfer* m_pInterruptTransfer[MAX_URB_NUMBER_IMU]; 65 | uint8_t m_uiInterruptBuffer[MAX_URB_NUMBER_IMU][32]; 66 | 67 | 68 | std::vector m_vecInterfaceNumberList; 69 | int m_iConfigurationValue; 70 | int m_iVideoEndpointAddress; 71 | int m_iIMUEndpointAddress; 72 | int m_iVideoTransMode; 73 | bool m_bRunning; 74 | 75 | std::thread m_threadProcessor; 76 | }; 77 | 78 | #endif // USBINTERFACE_H 79 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/eventproc/dataprocessthread.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef DATAPROCESSTHREAD_H 18 | #define DATAPROCESSTHREAD_H 19 | 20 | #include "../base/dataqueue.h" 21 | #include "../base/xthread.h" 22 | #include "celex5dataprocessor.h" 23 | #include "../include/celex5/celex5.h" 24 | 25 | class DataProcessThread : public XThread 26 | { 27 | public: 28 | DataProcessThread(const std::string& name); 29 | ~DataProcessThread(); 30 | 31 | void addData(uint8_t* data, uint32_t length, std::time_t timeStamp = 0); 32 | void addData(uint8_t* data, uint32_t length, std::vector imuData, time_t timeStamp); 33 | void addData(std::vector vecData); 34 | void clearData(); 35 | uint32_t queueSize(); 36 | uint32_t getPackageNo(); 37 | void setPackageNo(uint32_t no); 38 | void setDeviceType(CeleX5::DeviceType type); 39 | void setCeleX(CeleX5* pCeleX5); 40 | void setDataProcessor(CeleX5DataProcessor* pDataProcessor); 41 | void setIsPlayback(bool state); 42 | 43 | PlaybackState getPlaybackState(); 44 | void setPlaybackState(PlaybackState state); 45 | void setRecordState(bool bRecord); 46 | //Whether to display the images when recording 47 | void setShowImagesEnabled(bool enable); 48 | 49 | protected: 50 | void run() override; 51 | 52 | private: 53 | DataQueue m_queueData; 54 | CeleX5DataProcessor* m_pDataProcessor; 55 | std::queue> m_queueVecData; 56 | uint32_t m_uiPackageNo; 57 | CeleX5::DeviceType m_emDeviceType; 58 | CeleX5* m_pCeleX5; 59 | uint8_t* m_pMipiPackage; 60 | bool m_bPlaybackBinFile; 61 | PlaybackState m_emPlaybackState; 62 | std::vector m_vecIMUData; 63 | bool m_bRecordData; 64 | bool m_bShowImagesEnabled; 65 | }; 66 | 67 | #endif // DATAPROCESSTHREAD_H 68 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/sync_images.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef CELEX5_CALIBRATION_SRC_EXTRINSICS_SYNCIMAGES_H_ 22 | #define CELEX5_CALIBRATION_SRC_EXTRINSICS_SYNCIMAGES_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | class SyncImages { 39 | public: 40 | explicit SyncImages(const ros::NodeHandle &nh = ros::NodeHandle("~")); 41 | virtual ~SyncImages(); 42 | 43 | private: 44 | void SyncImagesCallback(const sensor_msgs::ImageConstPtr &frame_msg, 45 | const sensor_msgs::ImageConstPtr &events_msg); 46 | // void FrameImageCallback(const sensor_msgs::ImageConstPtr &frame_msg); 47 | // void EventsImageCallback(const sensor_msgs::ImageConstPtr &events_msg); 48 | 49 | ros::NodeHandle nh_; 50 | typedef message_filters::Subscriber MfImageSub; 51 | std::shared_ptr p_frame_sync_sub_; 52 | std::shared_ptr p_events_sync_sub_; 53 | 54 | std::shared_ptr p_ddyn_rec_; 55 | bool save_; 56 | // ros::Subscriber frame_sub_; 57 | // ros::Subscriber events_sub_; 58 | 59 | typedef message_filters::sync_policies::ApproximateTime SyncPolicy; 60 | std::shared_ptr> p_sync_; 61 | 62 | std::string store_dir_; 63 | }; 64 | 65 | #endif //CELEX5_CALIBRATION_SRC_EXTRINSICS_SYNCIMAGES_H_ 66 | -------------------------------------------------------------------------------- /celex5_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | celex5_msgs 4 | 0.0.0 5 | The celex5_msgs package 6 | 7 | 8 | 9 | 10 | hana 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | message_generation 41 | 42 | 43 | 44 | 45 | 46 | message_runtime 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/base/filedirectory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifdef _WIN32 18 | #include 19 | #else 20 | #include 21 | #endif 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include "filedirectory.h" 28 | 29 | FileDirectory::FileDirectory() 30 | { 31 | 32 | } 33 | 34 | FileDirectory::~FileDirectory() 35 | { 36 | 37 | } 38 | 39 | /* 40 | * @function: getApplicationDirPath 41 | * @brief : get the directory where exe is located 42 | * @input : 43 | * @output : 44 | * @return : the directory where exe is located 45 | */ 46 | std::string FileDirectory::getApplicationDirPath() 47 | { 48 | char path[1024]; 49 | memset(path, 0, 1024); 50 | #ifdef _WIN32 51 | GetModuleFileNameA(nullptr, path, 1024); 52 | std::string strPath = path; 53 | int len = strPath.find_last_of('\\'); 54 | return strPath.substr(0, len); 55 | #else 56 | int cnt = readlink("/proc/self/exe", path, 1024); 57 | std::cout << "FileDirectory::getApplicationDirPath: readlink count = " << cnt << std::endl; 58 | if (cnt < 0 || cnt >= 1024) 59 | { 60 | return std::string(""); 61 | } 62 | for (int i = cnt; i >= 0; --i) 63 | { 64 | if (path[i] == '/') 65 | { 66 | path[i + 1] = '\0'; 67 | break; 68 | } 69 | } 70 | return std::string(path); 71 | #endif 72 | } 73 | 74 | /* 75 | * @function: isFileExists 76 | * @brief : check if the given file exists 77 | * @input : filePath: a string of a file path and name 78 | * @output : 79 | * @return : true means the file exist, otherwise it doesn't exist 80 | */ 81 | bool FileDirectory::isFileExists(std::string filePath) 82 | { 83 | //printf("FileDirectory::isFileExists: %s", filePath.c_str()); 84 | std::fstream fsFile; 85 | fsFile.open(filePath.c_str(), std::ios::in); 86 | if (!fsFile.is_open()) 87 | { 88 | printf("%s can't find!\n", filePath.c_str()); 89 | return false; 90 | } 91 | fsFile.close(); 92 | return true; 93 | } 94 | -------------------------------------------------------------------------------- /celex5_calibration/src/temporal/temporal_offset_calculate.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef CELEX5_CALIBRATION_SRC_EXTRINSICS_EXTRINSICS_CALCULATE_H_ 22 | #define CELEX5_CALIBRATION_SRC_EXTRINSICS_EXTRINSICS_CALCULATE_H_ 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "matplotlib-cpp/matplotlibcpp.h" 37 | namespace plt = matplotlibcpp; 38 | 39 | class TemporalOffsetCalculate { 40 | public: 41 | explicit TemporalOffsetCalculate(const ros::NodeHandle &nh = ros::NodeHandle("~")); 42 | virtual ~TemporalOffsetCalculate(); 43 | 44 | private: 45 | void EventsCallback(const celex5_msgs::EventVectorConstPtr &msg); 46 | void FrameCallback(const sensor_msgs::ImageConstPtr &msg); 47 | 48 | /* 49 | * Return the intensity changes between frame2 and frame1 50 | */ 51 | static int64_t CalculateIntensityChanges(const cv::Mat &frame1, const cv::Mat &frame2); 52 | void AnimationPlot(); 53 | // std::vector FindCorners(const cv::Mat &image); 54 | 55 | ros::NodeHandle nh_; 56 | 57 | ros::Subscriber events_sub_; 58 | ros::Subscriber frame_sub_; 59 | 60 | std::shared_ptr p_ddyn_rec_; 61 | bool is_plot_; 62 | double x_length_; 63 | 64 | ros::Time init_stamp_; 65 | ros::Time last_events_stamp_; 66 | cv::Mat last_frame_; 67 | double last_events_rate_; 68 | double last_intensity_changes_; 69 | 70 | std::vector vec_events_rate_history_; 71 | std::vector vec_events_rate_stamps_; 72 | std::vector vec_intensity_changes_history_; 73 | std::vector vec_intensity_changes_stamps_; 74 | }; 75 | 76 | #endif //CELEX5_CALIBRATION_SRC_EXTRINSICS_EXTRINSICS_CALCULATE_H_ 77 | -------------------------------------------------------------------------------- /celex5_ros/src/bean/camera_publisher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | * 19 | * ------------------------------------------------------------ 20 | * Create this class because of the Segfault of image_transport. 21 | * gdb outputs: 22 | * ``` 23 | * ROS__GI___libc_free (mem=0x3e646e657065645f) at malloc.c:2951 24 | * 2951 malloc.c: No such file or directory. 25 | * ``` 26 | * Same problem can be found: 27 | * https://answers.ros.org/question/275710/the-node-with-pluginlib-can-not-run-in-custom-boost-situation/ 28 | * https://stackoverflow.com/questions/38429036/not-able-to-run-image-transport-from-cpp-code-segmentation-fault-core-dumped 29 | * https://answers.ros.org/question/275615/image_transportimagetransport-segfault/ 30 | * But my sysytem libboost version is 1.58, which is same to the pluginlib's dependence, 31 | * and there seems to `link to different version libboost` doesn't happened. 32 | * Still debuging... 33 | */ 34 | 35 | #ifndef CELEX5_ROS_SRC_BEAN_CAMERA_PUBLISHER_H_ 36 | #define CELEX5_ROS_SRC_BEAN_CAMERA_PUBLISHER_H_ 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace celex5_ros { 47 | class CameraPublisher { 48 | public: 49 | explicit CameraPublisher(const ros::NodeHandle &nh, 50 | int buffer_length = 10); 51 | CameraPublisher(const ros::NodeHandle &nh, 52 | std::string parameters_file_url, 53 | int buffer_length = 10); 54 | virtual ~CameraPublisher(); 55 | void Publish(const cv::Mat &image, 56 | const std::string &encoding, 57 | const std::string &frame_id = ""); 58 | bool IsSubscribed(); 59 | 60 | private: 61 | // std::string image_name_; 62 | int buffer_length_; 63 | 64 | std::string parameters_file_url_; 65 | 66 | ros::NodeHandle nh_; 67 | ros::Publisher image_pub_; 68 | ros::Publisher camera_info_pub_; 69 | 70 | std::shared_ptr p_camera_info_; 71 | }; 72 | }; 73 | 74 | #endif //CELEX5_ROS_SRC_BEAN_CAMERA_PUBLISHER_H_ 75 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/include/celex5/celex5datamanager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CELEX5_DATA_MANAGER_H 18 | #define CELEX5_DATA_MANAGER_H 19 | 20 | #include "celex5processeddata.h" 21 | 22 | /* 23 | * This is an abstract observer class. 24 | */ 25 | class CeleX5DataManager 26 | { 27 | public: 28 | enum emDataType { 29 | Default = 0, 30 | CeleX_Frame_Data = 1 31 | }; 32 | 33 | CeleX5DataManager() { } 34 | ~CeleX5DataManager() { } 35 | 36 | virtual void onFrameDataUpdated(CeleX5ProcessedData* data) = 0; 37 | }; 38 | 39 | /* 40 | * This is an abstract subject class. 41 | */ 42 | class CX5Subject 43 | { 44 | public: 45 | virtual void registerData(CeleX5DataManager* observer, CeleX5DataManager::emDataType type) = 0; 46 | virtual void unregisterData(CeleX5DataManager* observer, CeleX5DataManager::emDataType type) = 0; 47 | virtual void notify(CeleX5DataManager::emDataType dataType) = 0; 48 | }; 49 | 50 | /* 51 | * This is a concrete subject class(be observed by observer). 52 | */ 53 | class CX5SensorDataServer : public CX5Subject 54 | { 55 | public: 56 | CX5SensorDataServer() : m_pObserver(NULL), m_pCX5ProcessedData(NULL) 57 | { 58 | } 59 | virtual ~CX5SensorDataServer() 60 | { 61 | } 62 | /* 63 | * add observer 64 | */ 65 | void registerData(CeleX5DataManager* observer, CeleX5DataManager::emDataType type) 66 | { 67 | m_pObserver = observer; 68 | m_listDataType.push_back(type); 69 | } 70 | 71 | /* 72 | * delete observer 73 | */ 74 | void unregisterData(CeleX5DataManager* observer, CeleX5DataManager::emDataType type) 75 | { 76 | if (observer == m_pObserver) 77 | { 78 | m_listDataType.remove(type); 79 | } 80 | } 81 | 82 | /* 83 | * notify the observers to update 84 | */ 85 | void notify(CeleX5DataManager::emDataType dataType) 86 | { 87 | if (m_pObserver) 88 | { 89 | if (CeleX5DataManager::CeleX_Frame_Data == dataType) 90 | m_pObserver->onFrameDataUpdated(m_pCX5ProcessedData); 91 | } 92 | } 93 | inline void setCX5SensorData(CeleX5ProcessedData* data) { m_pCX5ProcessedData = data; } 94 | 95 | private: 96 | std::list m_listDataType; 97 | CeleX5DataManager* m_pObserver; 98 | CeleX5ProcessedData* m_pCX5ProcessedData; 99 | }; 100 | 101 | #endif // CELEX5_DATA_MANAGER_H 102 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/README_zh.md: -------------------------------------------------------------------------------- 1 | *[English Version](README.md)* 2 | 3 | # CeleX5-MIPI Intrinsics Calibration Demo 4 | 5 | > Calibrate CeleX5-MIPI camera **intrinsics** with `camera-calibration`. 6 | 7 | - [标定原理](#标定原理) 8 | - [运行](#运行) 9 | - [结果示例](#结果示例) 10 | 11 | 我们使用一个频闪的棋盘格标定板（可以用仓库中的这个[工具](../pattern/)来生成）来对CeleX5-MIPI事件相机进行内参标定。 12 | 13 | ## 标定原理 14 | 15 | 利用闪烁的棋盘格，控制CeleX5-MIPI相机生成棋盘格状的Event frame，然后通过传统的内参标定方法即可。这里采用ROS中集成的[`camera-calibration`](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration)标定工具进行标定，内部采用张正友标定法，标定结果的保存包含yaml文件、txt文件和用于最终标定结果的图像帧。 16 | 17 | ## 运行 18 | 19 | 1. 首先安装`camera-calibration`。 20 | 21 | ```bash 22 | sudo apt install ros-kinetic-camera-calibration 23 | ``` 24 | 25 | 2. 运行CeleX5-MIPI ROS节点： 26 | 27 | ```bash 28 | roslaunch celex5_ros celex5_ros_node.launch 29 | ``` 30 | 31 | 打开`rqt_reconfigure`，在**Event Off-Pixel Timestamp Mode**模式下，只保留`/celex5_mipi/display/binary_img`话题的输出，将其他的无关输出关闭。将`/celex5_mipi/display/binary_img`的帧率保持在20fps即可。过高可能会造成标定程序中的延迟。 32 | 33 | 3. 启动`camera-calibration`。各参数详细含义和更多用法请移步上面链接中的`camera-calibration`官方文档。 34 | 35 | ```bash 36 | rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.0212 image:=/celex5_mipi/display/binary_img 37 | ``` 38 | 39 | 其中参数含义： 40 | 41 | - `--size`：内点的数目（也就是出去边长最外圈方格数-1）的宽乘高。如果这个参数设置不对的话会在下面的标定步骤中发现标定程序毫无反应。 42 | - `--square`：标定板中的正方形的物理边长。单位为m。 43 | - `image:=`：监听的图像的话题名称。 44 | 45 | 运行成功后会有以下窗口： 46 | 47 | 48 | 49 | 移动闪烁棋盘格的屏幕，将右上角的X、Y、Size、Skew四个进度条都尽力变满变绿。X、Y顾名思义就是棋盘格在图像中的X方向和Y方向，Size是远近（棋盘格的大小）、Skew向上下左右的倾斜。具体详见`camera-calibration`官方文档。 50 | 51 | *Tips：移动过程中可以讲棋盘格图案暂停，从而避免在拖影严重时仍然进行角点检测。在固定好位置后再继续使得棋盘格闪烁，获得质量比较高的Event frame* 52 | 53 | 当你看到**CALIBRATE**按钮变绿后，点击经过计算即可获得内参标定结果。然后点击**SAVE**按钮即可保存标定结果。它会提示你保存路径，是个压缩包。解压后里面存储的有标定过程中用到的图像Frame和标定结果文件（一个yaml一个txt）。 54 | 55 | ## 结果示例 56 | 57 | 结果示例： 58 | 59 | 60 | ost.yaml： 61 | 62 | ```yaml 63 | image_width: 1280 64 | image_height: 800 65 | camera_name: narrow_stereo 66 | camera_matrix: 67 | rows: 3 68 | cols: 3 69 | data: [1687.514919, 0.000000, 676.131784, 0.000000, 1694.799737, 417.348889, 0.000000, 0.000000, 1.000000] 70 | distortion_model: plumb_bob 71 | distortion_coefficients: 72 | rows: 1 73 | cols: 5 74 | data: [-0.089225, -0.433622, 0.003022, -0.001306, 0.000000] 75 | rectification_matrix: 76 | rows: 3 77 | cols: 3 78 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 79 | projection_matrix: 80 | rows: 3 81 | cols: 4 82 | data: [1647.456299, 0.000000, 677.701102, 0.000000, 0.000000, 1683.463867, 418.507924, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 83 | ``` 84 | 85 | 过程中用到的图片样例： 86 | 87 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/configproc/tinyxml/tinystr.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | www.sourceforge.net/projects/tinyxml 3 | 4 | This software is provided 'as-is', without any express or implied 5 | warranty. In no event will the authors be held liable for any 6 | damages arising from the use of this software. 7 | 8 | Permission is granted to anyone to use this software for any 9 | purpose, including commercial applications, and to alter it and 10 | redistribute it freely, subject to the following restrictions: 11 | 12 | 1. The origin of this software must not be misrepresented; you must 13 | not claim that you wrote the original software. If you use this 14 | software in a product, an acknowledgment in the product documentation 15 | would be appreciated but is not required. 16 | 17 | 2. Altered source versions must be plainly marked as such, and 18 | must not be misrepresented as being the original software. 19 | 20 | 3. This notice may not be removed or altered from any source 21 | distribution. 22 | */ 23 | 24 | 25 | #ifndef TIXML_USE_STL 26 | 27 | #include "tinystr.h" 28 | 29 | // Error value for find primitive 30 | const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1); 31 | 32 | 33 | // Null rep. 34 | TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } }; 35 | 36 | 37 | void TiXmlString::reserve (size_type cap) 38 | { 39 | if (cap > capacity()) 40 | { 41 | TiXmlString tmp; 42 | tmp.init(length(), cap); 43 | memcpy(tmp.start(), data(), length()); 44 | swap(tmp); 45 | } 46 | } 47 | 48 | 49 | TiXmlString& TiXmlString::assign(const char* str, size_type len) 50 | { 51 | size_type cap = capacity(); 52 | if (len > cap || cap > 3*(len + 8)) 53 | { 54 | TiXmlString tmp; 55 | tmp.init(len); 56 | memcpy(tmp.start(), str, len); 57 | swap(tmp); 58 | } 59 | else 60 | { 61 | memmove(start(), str, len); 62 | set_size(len); 63 | } 64 | return *this; 65 | } 66 | 67 | 68 | TiXmlString& TiXmlString::append(const char* str, size_type len) 69 | { 70 | size_type newsize = length() + len; 71 | if (newsize > capacity()) 72 | { 73 | reserve (newsize + capacity()); 74 | } 75 | memmove(finish(), str, len); 76 | set_size(newsize); 77 | return *this; 78 | } 79 | 80 | 81 | TiXmlString operator + (const TiXmlString & a, const TiXmlString & b) 82 | { 83 | TiXmlString tmp; 84 | tmp.reserve(a.length() + b.length()); 85 | tmp += a; 86 | tmp += b; 87 | return tmp; 88 | } 89 | 90 | TiXmlString operator + (const TiXmlString & a, const char* b) 91 | { 92 | TiXmlString tmp; 93 | TiXmlString::size_type b_len = static_cast( strlen(b) ); 94 | tmp.reserve(a.length() + b_len); 95 | tmp += a; 96 | tmp.append(b, b_len); 97 | return tmp; 98 | } 99 | 100 | TiXmlString operator + (const char* a, const TiXmlString & b) 101 | { 102 | TiXmlString tmp; 103 | TiXmlString::size_type a_len = static_cast( strlen(a) ); 104 | tmp.reserve(a_len + b.length()); 105 | tmp.append(a, a_len); 106 | tmp += b; 107 | return tmp; 108 | } 109 | 110 | 111 | #endif // TIXML_USE_STL 112 | -------------------------------------------------------------------------------- /celex5_ros/src/interactive/celex5_display_controller.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef CELEX5_ROS_SRC_BEAN_CELEX5_DISPLAY_CONTROLLER_H_ 21 | #define CELEX5_ROS_SRC_BEAN_CELEX5_DISPLAY_CONTROLLER_H_ 22 | 23 | #include 24 | 25 | #include 26 | #include "celex5_display_pub_factory.h" 27 | #include 28 | 29 | namespace celex5_ros { 30 | 31 | typedef std::shared_ptr CeleX5DisplayPubFactoryPtr; 32 | 33 | class CeleX5DisplayController { 34 | public: 35 | static std::shared_ptr GetInstance(const ros::NodeHandle &nh, 36 | const std::shared_ptr &p_celex5_sensor); 37 | virtual ~CeleX5DisplayController(); 38 | void SetCeleX5Mode(CeleX5::CeleX5Mode mode); 39 | 40 | private: 41 | 42 | CeleX5DisplayController(const ros::NodeHandle &nh, 43 | std::shared_ptr p_celex5_sensor); 44 | CeleX5DisplayController(const CeleX5DisplayController &); 45 | CeleX5DisplayController &operator=(const CeleX5DisplayController &); 46 | 47 | static std::shared_ptr instance; 48 | static std::shared_ptr mutex_instance; 49 | 50 | void CloseAll(); 51 | void ChangeOptions(uint16_t mask); 52 | 53 | std::shared_ptr p_celex5_sensor_; 54 | std::shared_ptr p_ddyn_rec_; 55 | 56 | ros::NodeHandle nh_; 57 | 58 | CeleX5DisplayPubFactoryPtr p_binary_img_pub_; 59 | CeleX5DisplayPubFactoryPtr p_denoised_binary_img_pub_; 60 | CeleX5DisplayPubFactoryPtr p_count_img_pub_; 61 | CeleX5DisplayPubFactoryPtr p_optical_flow_img_pub_; 62 | CeleX5DisplayPubFactoryPtr p_accumulated_img_pub_; 63 | CeleX5DisplayPubFactoryPtr p_gray_img_pub_; 64 | CeleX5DisplayPubFactoryPtr p_superimposed_img_pub_; 65 | CeleX5DisplayPubFactoryPtr p_optical_flow_direction_img_pub_; 66 | CeleX5DisplayPubFactoryPtr p_optical_flow_speed_img_pub_; 67 | CeleX5DisplayPubFactoryPtr p_in_pixel_img_pub_; 68 | CeleX5DisplayPubFactoryPtr p_full_frame_img_pub_; 69 | 70 | std::unordered_map map_pub_mask; 71 | std::unordered_map map_mode_mask; 72 | }; 73 | } 74 | 75 | #endif //CELEX5_ROS_SRC_BEAN_CELEX5_DISPLAY_CONTROLLER_H_ 76 | -------------------------------------------------------------------------------- /celex5_calibration/src/pattern/README.md: -------------------------------------------------------------------------------- 1 | *[中文版本](README_zh.md)* 2 | 3 | # Calibration Pattern Generator 4 | 5 | > ROS node for generating **Blinking-Chessboard** calibration pattern for Event-based Camera(Dynamic Vision Sensor). 6 | 7 | - [Overview](#overview) 8 | - [Build and Run](#build-and-run) 9 | - [CeleX5-MIPI Intrinsics Calibration Demo](#celex5-mipi-intrinsics-calibration-demo) 10 | 11 | ## Overview 12 | 13 | An ROS node is provided to generate a **blinking chessboard** for calibrating the Event-based Camera(Dynamic Vision Sensor). It provides the support of `rqt_reconfigure` to control the size and blinking frequency of the chessboard. And provide to save it in ` .avi` video format. 14 | 15 | Chessboard 16 | 17 | 18 | 19 | ## Build and Run 20 | 21 | 1. Build 22 | 23 | If you have not yet compiled the entire repository, clone the entire repository and compile it (skip this step if you have already). 24 | 25 | ```bash 26 | mkdir -p ~/celex_ws/src 27 | cd ~/celex_ws/src 28 | git clone git@github.com:kehanXue/CeleX5-ROS.git 29 | # Or with http: `git clone https://github.com/kehanXue/CeleX5-ROS.git` 30 | cd .. 31 | rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 32 | catkin_make # Or use `catkin build` 33 | ``` 34 | 35 | 2. Run 36 | 37 | ```bash 38 | source ~/celex_ws/devel/setup.bash # Or source setup.zsh when you use zsh 39 | rosrun celex5_ros pattern_generator_node 40 | # In a new Terminal 41 | rosrun rqt_reconfigure rqt_reconfigure # Open rqt_reconfigure to config 42 | ``` 43 | 44 | Normal operation without error. You can see that a window will pop up showing the blinking chessboard. At this time, you can adjust the parameters of the blinking calibration board through the `rqt_reconfigure` panel: 45 | 46 | - `square_size`: the length of each side of the squares in a chessboard. In pixels. 47 | 48 | - `row_corners_num`和`col_corners_num`: the number of interior points of the rows and columns of the generated chessboard. 49 | 50 | - `blinking_fps`: the blinking frequency / frame rate of the chessboard. It is a cycle before one flash and the next flash. 51 | 52 | - `static_board`: controls whether the checkerboard blinks. 53 | 54 | - `record_video`: click start to generate the video of blinking chessboard, and click (uncheck) again to stop recording the video. The frequency of the chessboard blinking in the video is consistent with the current `blinking_fps` parameter. Note that adjusting the `blinking_fps` in the process of generating video is invalid for the generated video, and **doesn't adjust the size of the checkerboard during the video generation processing!** 55 | 56 | The video will be generated under the current application running path: 57 | 58 | 59 | 60 | 61 | ## CeleX5-MIPI Intrinsics Calibration Demo 62 | 63 | [CeleX5-MIPI Intrinsics Calibration Demo](../intrinsics) 64 | 65 | -------------------------------------------------------------------------------- /celex5_calibration/src/registration/event_registration.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #ifndef CELEX5_CALIBRATION_SRC_REGISTRATION_EVENT_REGISTRATION_H_ 22 | #define CELEX5_CALIBRATION_SRC_REGISTRATION_EVENT_REGISTRATION_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | class EventRegistration { 44 | public: 45 | explicit EventRegistration(const ros::NodeHandle &nh); 46 | virtual ~EventRegistration(); 47 | 48 | private: 49 | void SyncImagesCallback(const sensor_msgs::ImageConstPtr &depth_msg, 50 | const sensor_msgs::ImageConstPtr &rgb_msg); 51 | void EventsCallback(const celex5_msgs::EventVectorConstPtr &events_msg); 52 | void EventsCameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg); 53 | void RgbCameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg); 54 | 55 | ros::NodeHandle nh_; 56 | typedef message_filters::Subscriber MfImageSub; 57 | ros::Subscriber events_sub_; 58 | ros::Subscriber events_info_sub_; 59 | ros::Subscriber rgb_info_sub_; 60 | 61 | std::shared_ptr p_depth_image_sub_; 62 | std::shared_ptr p_rgb_image_sub_; 63 | 64 | typedef message_filters::sync_policies::ApproximateTime SyncPolicy; 65 | std::shared_ptr> p_sync_; 66 | 67 | cv::Mat rgb_frame_; 68 | cv::Mat depth_frame_; 69 | cv::Mat rgb_on_events_frame_; 70 | 71 | Eigen::Isometry3d T_; 72 | Eigen::Matrix3d rgb_K_; 73 | bool rgb_info_initialed; 74 | // Eigen::Matrix3d rgb_D_; 75 | Eigen::Matrix3d events_K_; 76 | cv::Mat events_D_; 77 | bool events_info_initialed; 78 | 79 | std::shared_ptr p_thread_process_; 80 | std::condition_variable cv_new_frame_; 81 | std::mutex mu_new_frame_; 82 | }; 83 | 84 | #endif //CELEX5_CALIBRATION_SRC_REGISTRATION_EVENT_REGISTRATION_H_ 85 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/configproc/wireincommand.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "wireincommand.h" 18 | #include 19 | 20 | #define INVALID_ADDR -1 21 | 22 | WireinCommand::WireinCommand(const std::string& name) 23 | : m_strName(name) 24 | , m_iAddressH(-1) 25 | , m_iAddressM(-1) 26 | , m_iAddressL(-1) 27 | , m_iValueH(-1) 28 | , m_iValueM(-1) 29 | , m_iValueL(-1) 30 | , m_iValue(-1) 31 | , m_iMinValue(0) 32 | , m_iMaxValue(0) 33 | { 34 | } 35 | 36 | WireinCommand::~WireinCommand() 37 | { 38 | } 39 | 40 | /* 41 | * @function : setValue 42 | * @brief : set the value according to the register type 43 | * @input : value : the value needs to be set 44 | * type : register type 45 | * @output : 46 | * @return : 47 | */ 48 | void WireinCommand::setValue(int16_t value, RegisterType type) 49 | { 50 | if (type == R_High) 51 | m_iValueH = value; 52 | else if (type == R_Middle) 53 | m_iValueM = value; 54 | else if (type == R_Low) 55 | m_iValueL = value; 56 | } 57 | 58 | /* 59 | * @function : combineRegBytes 60 | * @brief : set the value consisted of 24-bits data 61 | * @input : 62 | * @output : 63 | * @return : 64 | */ 65 | void WireinCommand::combineRegBytes() 66 | { 67 | if (INVALID_ADDR == m_iValueL) 68 | { 69 | m_iValue = m_iValueH; 70 | } 71 | else 72 | { 73 | if (INVALID_ADDR == m_iValueM) 74 | m_iValue = m_iValueL + (m_iValueH << 8); 75 | else 76 | m_iValue = m_iValueL + (m_iValueM << 8) + (m_iValueH << 16); 77 | } 78 | //std::cout << "---------- value = " << m_iValue << std::endl; 79 | } 80 | 81 | /* 82 | * @function : setAddress 83 | * @brief : set the address according to the register type 84 | * @input : address : the address needs to be set 85 | * type : register type 86 | * @output : 87 | * @return : 88 | */ 89 | void WireinCommand::setAddress(int16_t address, RegisterType type) 90 | { 91 | if (type == R_High) 92 | m_iAddressH = address; 93 | else if (type == R_Middle) 94 | m_iAddressM = address; 95 | else if (type == R_Low) 96 | m_iAddressL = address; 97 | } 98 | 99 | /* 100 | * @function : setMaxValue 101 | * @brief : set the maximum value 102 | * @input : value : the maximum value needs to be set 103 | * @output : 104 | * @return : 105 | */ 106 | void WireinCommand::setMaxValue(int32_t value) 107 | { 108 | m_iMaxValue = value; 109 | } 110 | 111 | /* 112 | * @function : setMinValue 113 | * @brief : set the minimum value 114 | * @input : value : the minimum value needs to be set 115 | * @output : 116 | * @return : 117 | */ 118 | void WireinCommand::setMinValue(int32_t value) 119 | { 120 | m_iMinValue = value; 121 | } 122 | -------------------------------------------------------------------------------- /celex5_ros/src/interactive/celex5_display_pub_factory.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef CELEX5_ROS_SRC_BEAN_CELEX5_DISPLAY_PUB_FACTORY_H_ 21 | #define CELEX5_ROS_SRC_BEAN_CELEX5_DISPLAY_PUB_FACTORY_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "bean/camera_publisher.h" 34 | 35 | #include "celex5/celex5.h" 36 | 37 | namespace celex5_ros { 38 | 39 | class CeleX5DisplayPubFactory { 40 | 41 | public: 42 | CeleX5DisplayPubFactory(const ros::NodeHandle &nh, 43 | std::string topic_name, 44 | std::shared_ptr p_celex5_sensor, 45 | std::shared_ptr p_ddyn_rec, 46 | CeleX5::EventPicType event_pic_type); 47 | CeleX5DisplayPubFactory(const ros::NodeHandle &nh, 48 | std::string topic_name, 49 | std::shared_ptr p_celex5_sensor, 50 | std::shared_ptr p_ddyn_rec, 51 | CeleX5::OpticalFlowPicType optical_flow_pic_type); 52 | CeleX5DisplayPubFactory(const ros::NodeHandle &nh, 53 | std::string topic_name, 54 | std::shared_ptr p_celex5_sensor, 55 | std::shared_ptr p_ddyn_rec, 56 | CeleX5::FullFramePicType full_frame_pic_type); 57 | 58 | virtual ~CeleX5DisplayPubFactory(); 59 | 60 | void Open(); 61 | void Close(); 62 | 63 | bool IsPublishEnable() const; 64 | 65 | private: 66 | 67 | static void ToColorOpticalMat(cv::Mat &optical_mat); 68 | 69 | std::shared_ptr p_celex5_sensor_; 70 | std::shared_ptr p_ddyn_rec_; 71 | 72 | ros::NodeHandle nh_; 73 | std::string frame_id_; 74 | std::shared_ptr p_publisher_; 75 | 76 | bool publish_enable_; 77 | std::shared_ptr p_mutex_; //TODO Delete 78 | std::shared_ptr publish_thread_; 79 | 80 | // CeleX5PicType celex5_pic_type_; 81 | CeleX5::EventPicType event_pic_type_; 82 | CeleX5::OpticalFlowPicType optical_flow_pic_type_; 83 | CeleX5::FullFramePicType full_frame_pic_type_; 84 | }; 85 | } 86 | 87 | #endif //CELEX5_ROS_SRC_BEAN_CELEX5_DISPLAY_PUB_FACTORY_H_ 88 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ## 2 | # The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | # 4 | # Copyright (C) 2020 Kehan.Xue 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # This program is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU General Public License 17 | # along with this program. If not, see . 18 | 19 | cmake_minimum_required(VERSION 3.2) 20 | project(celex5_sdk) 21 | 22 | set(CMAKE_CXX_STANDARD 14) 23 | 24 | #set(CMAKE_BUILD_TYPE "Release") 25 | #set(DEFINES -DCELEX_LIBRARY) 26 | set(CMAKE_C_FLAGS "-pipe -O2 -Wall -W -D_REENTRANT -fPIC") 27 | set(CMAKE_CXX_FLAGS "-pipe -O2 -std=gnu++0x -Wall -W -D_REENTRANT -fPIC") 28 | set(CMAKE_SHARED_LINKER_FLAGS "-Wl,-O1 -Wl,-rpath, -shared -Wl,-soname,libCeleX.so") 29 | 30 | find_package(OpenCV 3 REQUIRED) 31 | include_directories(${OpenCV_INCLUDE_DIRS}) 32 | 33 | 34 | include_directories(base) 35 | include_directories(configproc) 36 | include_directories(configproc/tinyxml) 37 | include_directories(cx3driver) 38 | include_directories(cx3driver/include) 39 | include_directories(eventproc) 40 | include_directories(include) 41 | include_directories(include/celex5) 42 | 43 | add_library(CeleX5 SHARED 44 | base/dataqueue.cpp 45 | base/dataqueue.h 46 | base/filedirectory.cpp 47 | base/filedirectory.h 48 | base/xthread.cpp 49 | base/xthread.h 50 | configproc/tinyxml/tinystr.cpp 51 | configproc/tinyxml/tinystr.h 52 | configproc/tinyxml/tinyxml.cpp 53 | configproc/tinyxml/tinyxml.h 54 | configproc/tinyxml/tinyxmlerror.cpp 55 | configproc/tinyxml/tinyxmlparser.cpp 56 | configproc/celex5cfgmgr.cpp 57 | configproc/celex5cfgmgr.h 58 | configproc/wireincommand.cpp 59 | configproc/wireincommand.h 60 | configproc/xmlreader.cpp 61 | configproc/xmlreader.h 62 | cx3driver/include/libusb.h 63 | cx3driver/bulktransfer.cpp 64 | cx3driver/bulktransfer.h 65 | cx3driver/celedriver.cpp 66 | cx3driver/celedriver.h 67 | cx3driver/cypress.cpp 68 | cx3driver/cypress.h 69 | cx3driver/package.cpp 70 | cx3driver/package.h 71 | cx3driver/usbinterface.cpp 72 | cx3driver/usbinterface.h 73 | eventproc/celex5.cpp 74 | eventproc/celex5dataprocessor.cpp 75 | eventproc/celex5dataprocessor.h 76 | eventproc/celex5processeddata.cpp 77 | eventproc/dataprocessthread.cpp 78 | eventproc/dataprocessthread.h 79 | eventproc/datarecorder.cpp 80 | eventproc/datarecorder.h 81 | include/celex5/celex5.h 82 | include/celex5/celex5datamanager.h 83 | include/celex5/celex5processeddata.h 84 | include/celextypes.h) 85 | 86 | target_link_libraries(CeleX5 87 | -lpthread 88 | -L/usr/lib/x86_64-linux-gnu/ 89 | -lusb-1.0 90 | ${OpenCV_LIBS} 91 | ) 92 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/configproc/celex5cfgmgr.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include "celex5cfgmgr.h" 20 | #include "tinyxml/tinyxml.h" 21 | #include "../include/celextypes.h" 22 | #include "wireincommand.h" 23 | #include "xmlreader.h" 24 | 25 | CeleX5CfgMgr::CeleX5CfgMgr() 26 | { 27 | } 28 | 29 | CeleX5CfgMgr::~CeleX5CfgMgr() 30 | { 31 | } 32 | 33 | /* 34 | * @function : parseCeleX5Cfg 35 | * @brief : parse the config file of CeleX5 36 | * @input : cfgName : name of the config file that needs to be parsed 37 | * @output : 38 | * @return : bool : true for parsing sucessfully; false for failed 39 | */ 40 | bool CeleX5CfgMgr::parseCeleX5Cfg(const std::string& cfgName) 41 | { 42 | XmlReader xml; 43 | TiXmlDocument dom; 44 | std::vector pCmdList; 45 | std::string name = ""; 46 | if (xml.parse(cfgName, &dom)) 47 | { 48 | if (xml.importCeleX5Commands(m_CommandListCeleX5, &dom)) 49 | { 50 | for (auto itr = m_CommandListCeleX5.begin(); itr != m_CommandListCeleX5.end(); ++itr) 51 | { 52 | //cout << "CeleX5CfgMgr::parseCeleX5Cfg: " << itr->first << endl; 53 | pCmdList = itr->second; 54 | for (auto cmd = pCmdList.begin(); cmd != pCmdList.end(); ++cmd) 55 | { 56 | name = (*cmd)->getName(); 57 | //cout << "----- Register Name: " << name << endl; 58 | if (m_CommandMapCeleX5.find(name) == m_CommandMapCeleX5.end()) 59 | { 60 | m_CommandMapCeleX5[name] = *cmd; 61 | } 62 | else 63 | { 64 | std::cout << "More than one command has the same name: " << name << std::endl; 65 | } 66 | } 67 | } 68 | return true; 69 | } 70 | } 71 | return false; 72 | } 73 | 74 | /* 75 | * @function : saveCeleX5XML 76 | * @brief : saves the config file after modification 77 | * @input : mapCfgInfo : parameters in the config file 78 | * @output : 79 | * @return : bool : true for saving sucessfully; false for failed 80 | */ 81 | bool CeleX5CfgMgr::saveCeleX5XML(std::map>& mapCfgInfo) 82 | { 83 | XmlReader xml; 84 | return xml.saveXML(mapCfgInfo); 85 | } 86 | 87 | /* 88 | * @function : saveCeleX5XML 89 | * @brief : save the config file if the xml file not exists when sensor first launchs 90 | * @input : cfgName : name of the config file that needs to be saved 91 | * @output : 92 | * @return : bool : true for saving sucessfully; false for failed 93 | */ 94 | bool CeleX5CfgMgr::saveCeleX5XML(const std::string& cfgName) 95 | { 96 | XmlReader xml; 97 | return xml.saveXML(cfgName); 98 | } 99 | 100 | /* 101 | * @function : getCeleX5Cfg 102 | * @brief : get the parameters in the config file 103 | * @input : 104 | * @output : 105 | * @return : key-value type parameters in the config file 106 | */ 107 | std::map> CeleX5CfgMgr::getCeleX5Cfg() 108 | { 109 | return m_CommandListCeleX5; 110 | } 111 | -------------------------------------------------------------------------------- /celex5_ros/src/bean/camera_publisher.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #include 21 | #include 22 | #include "camera_publisher.h" 23 | 24 | celex5_ros::CameraPublisher::CameraPublisher(const ros::NodeHandle &nh, 25 | int buffer_length) 26 | : buffer_length_(buffer_length), 27 | nh_(nh) { 28 | 29 | image_pub_ = nh_.advertise("raw_image", buffer_length_); 30 | camera_info_pub_ = nh_.advertise("camera_info", buffer_length_); 31 | 32 | ros::NodeHandle nh_root("~"); 33 | std::string camera_cfg_path("./"); 34 | nh_root.param("sensor_cfg_file_dir", camera_cfg_path, camera_cfg_path); 35 | parameters_file_url_ = "file://" + camera_cfg_path + "celex5_events_parameters.yaml"; 36 | p_camera_info_ = 37 | std::make_shared(nh_, "narrow_stereo", 38 | parameters_file_url_); 39 | } 40 | 41 | celex5_ros::CameraPublisher::CameraPublisher(const ros::NodeHandle &nh, 42 | std::string parameters_file_url, 43 | int buffer_length) 44 | : buffer_length_(buffer_length), 45 | parameters_file_url_(std::move(parameters_file_url)), 46 | nh_(nh) { 47 | image_pub_ = nh_.advertise("raw_image", buffer_length_); 48 | camera_info_pub_ = nh_.advertise("camera_info", buffer_length_); 49 | 50 | p_camera_info_ = 51 | std::make_shared(nh_, "narrow_stereo", 52 | parameters_file_url_); 53 | } 54 | 55 | celex5_ros::CameraPublisher::~CameraPublisher() = default; 56 | 57 | void celex5_ros::CameraPublisher::Publish(const cv::Mat &image, 58 | const std::string &encoding, 59 | const std::string &frame_id) { 60 | if (IsSubscribed()) { 61 | ros::Time now_time = ros::Time::now(); 62 | 63 | sensor_msgs::ImagePtr image_ptr_msg = 64 | cv_bridge::CvImage(std_msgs::Header(), encoding, image).toImageMsg(); 65 | image_ptr_msg->header.stamp = now_time; 66 | image_ptr_msg->header.frame_id = frame_id; 67 | 68 | sensor_msgs::CameraInfoPtr 69 | camera_info_ptr_msg = boost::make_shared(p_camera_info_->getCameraInfo()); 70 | camera_info_ptr_msg->header.stamp = now_time; 71 | camera_info_ptr_msg->header.frame_id = frame_id; 72 | 73 | image_pub_.publish(image_ptr_msg); 74 | camera_info_pub_.publish(camera_info_ptr_msg); 75 | } 76 | } 77 | 78 | bool celex5_ros::CameraPublisher::IsSubscribed() { 79 | return image_pub_.getNumSubscribers() > 0 || camera_info_pub_.getNumSubscribers() > 0; 80 | } 81 | -------------------------------------------------------------------------------- /celex5_calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | celex5_calibration 4 | 0.0.0 5 | The celex5_calibration ROS package, used to calibrate camera parameters for CeleX5-MIPI Event-based Camera 6 | 7 | 8 | 9 | 10 | kehan 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | message_filters 53 | roscpp 54 | sensor_msgs 55 | cv_bridge 56 | ddynamic_reconfigure 57 | celex5_msgs 58 | message_filters 59 | roscpp 60 | sensor_msgs 61 | cv_bridge 62 | ddynamic_reconfigure 63 | celex5_msgs 64 | message_filters 65 | roscpp 66 | sensor_msgs 67 | cv_bridge 68 | ddynamic_reconfigure 69 | celex5_msgs 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /celex5_ros/src/interactive/celex5_ddy_configure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef CELEX5_ROS_SRC_INTERFACE_CELEX5_CONFIGURE_H_ 21 | #define CELEX5_ROS_SRC_INTERFACE_CELEX5_CONFIGURE_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include "celex5_display_controller.h" 31 | #include "bean/celex5_options.h" 32 | 33 | namespace celex5_ros { 34 | class CeleX5DdyConfigure { 35 | 36 | public: 37 | explicit CeleX5DdyConfigure(std::shared_ptr p_celex5_sensor, 38 | const ros::NodeHandle &nh = ros::NodeHandle("~")); 39 | virtual ~CeleX5DdyConfigure(); 40 | void UpdateCeleX5AllOptions(); 41 | void PublishReconfigureServices(); 42 | 43 | /* 44 | * Function tools to get parameters from ROS Param Server 45 | */ 46 | static void ReadROSParam(const ros::NodeHandle &nh, 47 | const std::string ¶m_name, 48 | uint32_t ¶m); 49 | static void ReadROSParam(const ros::NodeHandle &nh, 50 | const std::string ¶m_name, 51 | int ¶m); 52 | static void ReadROSParam(const ros::NodeHandle &nh, 53 | const std::string ¶m_name, 54 | bool ¶m); 55 | static void ReadROSParam(const ros::NodeHandle &nh, 56 | const std::string ¶m_name, 57 | std::string ¶m); 58 | private: 59 | ros::NodeHandle nh_; 60 | std::shared_ptr p_ddyn_rec_; 61 | std::shared_ptr p_celex5_sensor_; 62 | std::shared_ptr p_celex5_options_; 63 | 64 | /* 65 | * Parameters reconfigure callback 66 | */ 67 | void ParamFixedModeCb(int fixed_mode); 68 | void ParamLoopMode1Cb(int loop_mode1); 69 | void ParamLoopMode2Cb(int loop_mode2); 70 | void ParamLoopMode3Cb(int loop_mode3); 71 | 72 | void ParamEventFrameTimeCb(int new_event_frame_time); 73 | void ParamOpticalFlowFrameTimeCb(int new_optical_flow_frame_time); 74 | 75 | void ParamThresholdCb(int new_threshold); 76 | void ParamBrightnessCb(int new_brightness); 77 | void ParamISOLevelCb(int new_iso_level); 78 | void ParamRawEventsEnabledCb(bool new_raw_events_status); 79 | void ParamPolarityImgEnabledCb(bool new_raw_events_status); 80 | void ParamImuEnabledCb(bool new_imu_status); 81 | // void ParamContrastCb(int new_contrast); 82 | void ParamClockRateCb(int new_clock_rate); 83 | void ParamLoopModeEnabledCb(bool new_loop_mode_status); 84 | void ParamEventDurationInLoopCb(int new_event_duration_in_loop); 85 | void ParamPictureNumberInLoopCb(int new_picture_number_in_loop); 86 | 87 | void ParamEventFpnFilePathCb(const std::string &new_fpn_file_path); 88 | void ParamFrameFpnFilePathCb(const std::string &new_fpn_file_path); 89 | }; 90 | } 91 | 92 | #endif //CELEX5_ROS_SRC_INTERFACE_CELEX5_CONFIGURE_H_ 93 | -------------------------------------------------------------------------------- /celex5_ros/src/bean/celex5_data_forwarder.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef CELEX5_ROS_SRC_BEAN_CELEX5_DATA_FORWARDER_H_ 21 | #define CELEX5_ROS_SRC_BEAN_CELEX5_DATA_FORWARDER_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "celex5_msgs/Event.h" 39 | #include "celex5_msgs/EventVector.h" 40 | #include "celex5_msgs/Imu.h" 41 | #include "celex5_msgs/ImuVector.h" 42 | 43 | #include "interactive/celex5_ddy_configure.h" 44 | #include "bean/celex5_options.h" 45 | #include "bean/camera_publisher.h" 46 | 47 | #include "celex5/celex5datamanager.h" 48 | 49 | namespace celex5_ros { 50 | 51 | const int CELEX5_MAT_ROWS = 800; 52 | const int CELEX5_MAT_COLS = 1280; 53 | 54 | class CeleX5DataForwarder : public CeleX5DataManager { 55 | 56 | public: 57 | explicit CeleX5DataForwarder(const ros::NodeHandle &nh, 58 | const std::shared_ptr &p_celex5_sensor); 59 | virtual ~CeleX5DataForwarder(); 60 | 61 | private: 62 | void onFrameDataUpdated(CeleX5ProcessedData *processed_data) override; 63 | void CreatePubThreads(); 64 | void CreateRawEventsPubThread(); 65 | void CreatePolarityImgPubThread(); 66 | void CreateImuDataPubThread(); 67 | 68 | std::condition_variable cv_raw_events_; 69 | std::condition_variable cv_polarity_img_; 70 | std::condition_variable cv_imu_data_; 71 | std::mutex mu_raw_events_; 72 | std::mutex mu_polarity_img_; 73 | std::mutex mu_imu_data_; 74 | 75 | std::vector vec_events_; 76 | std::vector vec_raw_events_; 77 | std::vector vec_polarity_events_; 78 | 79 | std::shared_ptr p_raw_events_pub_thread_; 80 | std::shared_ptr p_polarity_img_pub_thread_; 81 | std::shared_ptr p_imu_data_pub_thread_; 82 | 83 | ros::NodeHandle nh_; 84 | std::string frame_id_; 85 | 86 | std::shared_ptr p_celex5_sensor_; 87 | std::shared_ptr p_celex5_options_; 88 | 89 | CX5SensorDataServer *p_celex5_data_server_; 90 | 91 | ros::Publisher events_pub_; 92 | ros::Publisher celex_imu_pub_; 93 | ros::Publisher imu_pub_; 94 | 95 | std::shared_ptr p_polarity_img_pub_; 96 | // ros::Publisher binary_img_pub_; 97 | // ros::Publisher denoised_img_pub_; 98 | // ros::Publisher count_img_pub_; 99 | // ros::Publisher optical_flow_img_pub_; 100 | // 101 | // ros::Publisher gray_img_pub_; 102 | // ros::Publisher accumulated_img_pub_; 103 | // ros::Publisher superimposed_img_pub_; 104 | // ros::Publisher optical_flow_direction_img_pub_; 105 | // ros::Publisher optical_flow_speed_img_pub_; 106 | }; 107 | } 108 | 109 | #endif //CELEX5_ROS_SRC_BEAN_CELEX5_DATA_FORWARDER_H_ 110 | -------------------------------------------------------------------------------- /celex5_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | celex5_ros 4 | 1.1.0 5 | The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 6 | 7 | 8 | 9 | 10 | kehan 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | cv_bridge 53 | geometry_msgs 54 | roscpp 55 | std_msgs 56 | ddynamic_reconfigure 57 | nodelet 58 | celex5_msgs 59 | sensor_msgs 60 | cv_bridge 61 | geometry_msgs 62 | roscpp 63 | std_msgs 64 | ddynamic_reconfigure 65 | nodelet 66 | celex5_msgs 67 | sensor_msgs 68 | cv_bridge 69 | cv_bridge 70 | geometry_msgs 71 | roscpp 72 | ddynamic_reconfigure 73 | nodelet 74 | celex5_msgs 75 | sensor_msgs 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/cx3driver/package.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include "package.h" 19 | 20 | CElement::CElement() 21 | { 22 | wdataLen = 0; 23 | } 24 | 25 | CElement::~CElement() 26 | { 27 | 28 | } 29 | 30 | uint8_t *CElement::begin() 31 | { 32 | return data; 33 | } 34 | uint8_t *CElement::end() 35 | { 36 | return data + wdataLen; 37 | } 38 | 39 | void CElement::save(uint8_t *buffer, uint16_t wLen) 40 | { 41 | if (wLen > MAX_ELEMENT_BUFFER_SIZE) 42 | wLen = MAX_ELEMENT_BUFFER_SIZE; 43 | memcpy(data, buffer, wLen); 44 | wdataLen = wLen; 45 | } 46 | 47 | CPackage::CPackage() 48 | { 49 | m_emStatus = BUFFER_STATUS_EMPTY; 50 | m_uiOffset = 0; 51 | m_lTimestampEnd = 0; 52 | #ifdef NOT_USE_VECTOR 53 | m_pPackageBuffer = new uint8_t[MAX_ELEMENT_BUFFER_SIZE * 50]; 54 | #else 55 | for (int i = 0; i < 50; i++) 56 | { 57 | CElement *element = new CElement(); 58 | if (element) 59 | { 60 | m_vecElementList.push_back(element); 61 | } 62 | } 63 | #endif // _NOT_USE_VECTOR_ 64 | } 65 | 66 | CPackage::~CPackage() 67 | { 68 | #ifdef NOT_USE_VECTOR 69 | if (m_pPackageBuffer != nullptr) 70 | { 71 | delete[] m_pPackageBuffer; 72 | m_pPackageBuffer = nullptr; 73 | } 74 | #else 75 | while (!m_vecElementList.empty()) 76 | { 77 | CElement *element = *m_vecElementList.begin(); 78 | m_vecElementList.erase(m_vecElementList.begin()); 79 | delete element; 80 | } 81 | #endif 82 | } 83 | 84 | void CPackage::insert(uint8_t *buffer, uint16_t wLen) 85 | { 86 | #ifdef NOT_USE_VECTOR 87 | if (m_uiOffset < MAX_ELEMENT_BUFFER_SIZE * 50 - wLen) 88 | { 89 | memcpy(m_pPackageBuffer + m_uiOffset, buffer, wLen); 90 | m_uiOffset += wLen; 91 | } 92 | else 93 | { 94 | //printf("-------------- CPackage::insert wrong data --------------\n"); 95 | } 96 | #else 97 | if (m_uiOffset < m_vecElementList.size()) 98 | { 99 | m_vecElementList[m_uiOffset]->save(buffer, wLen); 100 | m_uiOffset++; 101 | } 102 | else 103 | { 104 | //("-------------- CPackage::insert wrong data --------------\n"); 105 | } 106 | #endif 107 | } 108 | 109 | void CPackage::end(void) 110 | { 111 | m_emStatus = BUFFER_STATUS_FULL; 112 | } 113 | 114 | void CPackage::clearData() 115 | { 116 | m_emStatus = BUFFER_STATUS_EMPTY; 117 | m_uiOffset = 0; 118 | } 119 | 120 | int CPackage::size() 121 | { 122 | return m_uiOffset; 123 | } 124 | 125 | bool CPackage::getImage(std::vector &image) 126 | { 127 | image.clear(); 128 | if (m_emStatus == BUFFER_STATUS_FULL) 129 | { 130 | for (size_t i = 0; i < m_uiOffset; i++) 131 | { 132 | image.insert(image.end(), m_vecElementList[i]->begin(), m_vecElementList[i]->end()); 133 | } 134 | m_uiOffset = 0; 135 | m_emStatus = BUFFER_STATUS_EMPTY; 136 | return true; 137 | } 138 | return false; 139 | } 140 | 141 | bool CPackage::getData(uint8_t* pData, uint32_t& length) 142 | { 143 | if (nullptr == pData) 144 | return false; 145 | if (m_emStatus == BUFFER_STATUS_FULL) 146 | { 147 | #ifdef NOT_USE_VECTOR 148 | if (m_uiOffset > 1536001) 149 | { 150 | m_uiOffset = 0; 151 | m_emStatus = BUFFER_STATUS_EMPTY; 152 | //printf("----------------- wrong data size = %d\n", m_uiOffset); 153 | return false; 154 | } 155 | memcpy(pData, m_pPackageBuffer, m_uiOffset); 156 | length = m_uiOffset; 157 | #else 158 | int offSize = 0; 159 | int dataSize = 0; 160 | for (size_t i = 0; i < m_uiOffset; i++) 161 | { 162 | dataSize = m_vecElementList[i]->end() - m_vecElementList[i]->begin(); 163 | if (offSize + dataSize > 1536001) 164 | { 165 | m_uiOffset = 0; 166 | m_emStatus = BUFFER_STATUS_EMPTY; 167 | //printf("----------------- wrong data size = %d\n", offSize + dataSize); 168 | return false; 169 | } 170 | memcpy(pData + offSize, m_vecElementList[i]->begin(), dataSize); 171 | offSize += dataSize; 172 | } 173 | length = offSize; 174 | #endif 175 | //printf("------------------- length = %d\n", length); 176 | m_uiOffset = 0; 177 | m_emStatus = BUFFER_STATUS_EMPTY; 178 | return true; 179 | } 180 | return false; 181 | } -------------------------------------------------------------------------------- /celex5_ros/launch/celex5_ros_node.launch: -------------------------------------------------------------------------------- 1 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/base/xthread.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include "xthread.h" 19 | 20 | XThread::XThread(const std::string threadName) 21 | : m_threadName(threadName) 22 | , m_threadID(0) 23 | , m_bRun(false) 24 | , m_bSuspended(false) 25 | #ifndef _WIN32 26 | , m_mutex(PTHREAD_MUTEX_INITIALIZER) 27 | , m_cond(PTHREAD_COND_INITIALIZER) 28 | #endif 29 | { 30 | 31 | } 32 | 33 | XThread::~XThread() 34 | { 35 | 36 | } 37 | 38 | //Create a thread and run (default) or hang 39 | bool XThread::start(bool bSuspend/* = false*/) 40 | { 41 | m_bRun = createThread(bSuspend); 42 | return m_bRun; 43 | } 44 | 45 | //Create a thread and run (default) or hang 46 | bool XThread::createThread(bool bSuspend/* = false*/) 47 | { 48 | if(!m_bRun) 49 | { 50 | #ifdef _WIN32 51 | if(bSuspend) 52 | { 53 | m_handle = (HANDLE)_beginthreadex(NULL, 0, staticThreadFunc, this, CREATE_SUSPENDED, &m_threadID); 54 | m_bSuspended = true; 55 | } 56 | else 57 | { 58 | m_handle = (HANDLE)_beginthreadex(NULL, 0, staticThreadFunc, this, 0, &m_threadID); 59 | } 60 | m_bRun = (NULL != m_handle); 61 | #else 62 | int status = pthread_create(&m_threadID, NULL, staticThreadFunc, this); 63 | if (status != 0) 64 | std::cout << "creating thread failure" << std::endl; 65 | else 66 | m_bRun = true; 67 | #endif 68 | } 69 | return m_bRun; 70 | } 71 | 72 | //If the waiting time (milliseconds) is negative, it means unlimited wait. 73 | void XThread::join(int timeout/* = -1*/) 74 | { 75 | #ifdef _WIN32 76 | if(m_handle && m_bRun) 77 | { 78 | if(timeout < 0) 79 | timeout = INFINITE; 80 | ::WaitForSingleObject(m_handle, timeout); 81 | } 82 | #else 83 | #endif 84 | } 85 | 86 | //Resume the suspended thread 87 | void XThread::resume() 88 | { 89 | std::cout << "XThread::resume: m_bRun = " << m_bRun << ", m_bSuspended = " << m_bSuspended << std::endl; 90 | if (m_bRun && m_bSuspended) 91 | { 92 | std::cout << "XThread::resume" << std::endl; 93 | #ifdef _WIN32 94 | ::ResumeThread(m_handle); 95 | m_bSuspended = false; 96 | #else 97 | pthread_mutex_lock(&m_mutex); 98 | m_bSuspended = false; 99 | pthread_cond_signal(&m_cond); 100 | pthread_mutex_unlock(&m_mutex); 101 | #endif 102 | } 103 | } 104 | 105 | //Suspend the thread 106 | void XThread::suspend() 107 | { 108 | if (m_bRun && !m_bSuspended) 109 | { 110 | std::cout << "XThread::suspend" << std::endl; 111 | #ifdef _WIN32 112 | m_bSuspended = true; 113 | ::SuspendThread(m_handle); 114 | #else 115 | pthread_mutex_lock(&m_mutex); 116 | m_bSuspended = true; 117 | pthread_mutex_unlock(&m_mutex); 118 | #endif 119 | } 120 | } 121 | 122 | //Terminate the thread 123 | bool XThread::terminate() 124 | { 125 | #ifdef _WIN32 126 | unsigned long exitCode = 0; 127 | if(m_handle && m_bRun) 128 | { 129 | if (::TerminateThread(m_handle, exitCode)) 130 | { 131 | ::CloseHandle(m_handle); 132 | m_handle = NULL; 133 | m_bRun = false; 134 | return true; 135 | } 136 | printf("XThread::terminate: exitCode = %ld\n", exitCode); 137 | } 138 | #else 139 | #endif 140 | return false; 141 | } 142 | 143 | bool XThread::isRunning() 144 | { 145 | return m_bRun; 146 | } 147 | 148 | unsigned int XThread::getThreadID() 149 | { 150 | return m_threadID; 151 | } 152 | 153 | std::string XThread::getThreadName() 154 | { 155 | return m_threadName; 156 | } 157 | 158 | void XThread::setThreadName(std::string threadName) 159 | { 160 | m_threadName = threadName; 161 | } 162 | 163 | #ifdef _WIN32 164 | //Thread function 165 | unsigned int XThread::staticThreadFunc(void* arg) 166 | { 167 | XThread* pThread = (XThread*)arg; //Get the thread class pointer 168 | pThread->run(); 169 | 170 | return 0; 171 | } 172 | #else 173 | //Thread function 174 | void* XThread::staticThreadFunc(void* args) 175 | { 176 | XThread* pThread = static_cast(args); //Get the thread class pointer 177 | if (pThread) 178 | pThread->run(); 179 | return NULL; 180 | } 181 | #endif 182 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/eventproc/dataprocessthread.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include "dataprocessthread.h" 19 | 20 | DataProcessThread::DataProcessThread(const std::string &name) 21 | : XThread(name) 22 | , m_uiPackageNo(0) 23 | , m_emDeviceType(CeleX5::Unknown_Devive) 24 | , m_bPlaybackBinFile(false) 25 | , m_emPlaybackState(NoBinPlaying) 26 | , m_bRecordData(false) 27 | , m_bShowImagesEnabled(true) 28 | { 29 | m_pMipiPackage = new uint8_t[1536001]; 30 | } 31 | 32 | DataProcessThread::~DataProcessThread() 33 | { 34 | delete[] m_pMipiPackage; 35 | m_pMipiPackage = nullptr; 36 | } 37 | 38 | void DataProcessThread::addData(uint8_t* data, uint32_t length, time_t timeStamp) 39 | { 40 | m_queueData.push(data, length, timeStamp); 41 | } 42 | 43 | void DataProcessThread::addData(uint8_t* data, uint32_t length, std::vector imuData, std::time_t timeStamp) 44 | { 45 | m_queueData.push(data, length, imuData, timeStamp); 46 | } 47 | 48 | void DataProcessThread::addData(std::vector vecData) 49 | { 50 | //cout << "size = " << vecData.size() << endl; 51 | if (vecData.size() > 0) 52 | { 53 | std::vector vec; 54 | vec.swap(vecData); 55 | //cout << "DataProcessThread::addData = " << vec.size() << endl; 56 | m_queueVecData.push(vec); 57 | } 58 | //cout << m_queueVecData.size() << endl; 59 | } 60 | 61 | void DataProcessThread::clearData() 62 | { 63 | m_queueData.clear(); 64 | m_uiPackageNo = 0; 65 | } 66 | 67 | uint32_t DataProcessThread::queueSize() 68 | { 69 | return m_queueData.size(); 70 | } 71 | 72 | uint32_t DataProcessThread::getPackageNo() 73 | { 74 | return m_uiPackageNo; 75 | } 76 | 77 | void DataProcessThread::setPackageNo(uint32_t no) 78 | { 79 | m_uiPackageNo = no; 80 | } 81 | 82 | void DataProcessThread::setDeviceType(CeleX5::DeviceType type) 83 | { 84 | m_emDeviceType = type; 85 | } 86 | 87 | void DataProcessThread::setCeleX(CeleX5* pCeleX5) 88 | { 89 | m_pCeleX5 = pCeleX5; 90 | } 91 | 92 | void DataProcessThread::setDataProcessor(CeleX5DataProcessor* pDataProcessor) 93 | { 94 | m_pDataProcessor = pDataProcessor; 95 | } 96 | 97 | void DataProcessThread::setIsPlayback(bool state) 98 | { 99 | m_bPlaybackBinFile = state; 100 | } 101 | 102 | PlaybackState DataProcessThread::getPlaybackState() 103 | { 104 | return m_emPlaybackState; 105 | } 106 | 107 | void DataProcessThread::setPlaybackState(PlaybackState state) 108 | { 109 | m_emPlaybackState = state; 110 | } 111 | 112 | void DataProcessThread::setRecordState(bool bRecord) 113 | { 114 | m_bRecordData = bRecord; 115 | } 116 | 117 | void DataProcessThread::setShowImagesEnabled(bool enable) 118 | { 119 | m_bShowImagesEnabled = enable; 120 | } 121 | 122 | void DataProcessThread::run() 123 | { 124 | while (m_bRun) 125 | { 126 | //cout << "---------- DataProcessThread::run ----------" << endl; 127 | #ifndef _WIN32 128 | pthread_mutex_lock(&m_mutex); 129 | while (m_bSuspended) 130 | { 131 | pthread_cond_wait(&m_cond, &m_mutex); 132 | } 133 | pthread_mutex_unlock(&m_mutex); 134 | #endif 135 | if (m_bPlaybackBinFile) //for playback 136 | { 137 | if (m_queueData.size() > 0) 138 | { 139 | uint32_t dataLen = 0; 140 | time_t timestamp = 0; 141 | m_queueData.pop(m_pMipiPackage, &dataLen, m_vecIMUData, ×tamp); 142 | //std::cout << "------------------" << "pop data size = " << dataLen << std::endl; 143 | if (dataLen > 0) 144 | { 145 | m_pDataProcessor->processMIPIData(m_pMipiPackage, dataLen, timestamp, m_vecIMUData); 146 | //cout << __FUNCTION__ << ": imu size = " << m_vecIMUData.size() << endl; 147 | if (m_vecIMUData.size() > 0) 148 | m_vecIMUData.clear(); 149 | m_uiPackageNo++; 150 | } 151 | } 152 | else 153 | { 154 | if (m_emPlaybackState == BinReadFinished) 155 | { 156 | m_emPlaybackState = PlayFinished; 157 | } 158 | } 159 | } 160 | else //--- for real display --- 161 | { 162 | std::time_t time_stamp_end = 0; 163 | std::vector imu_data; 164 | uint32_t dataLen = 0; 165 | m_pCeleX5->getCeleXRawData(m_pMipiPackage, dataLen, time_stamp_end, imu_data); 166 | if (dataLen > 0) 167 | { 168 | //std::cout << "dataLen = " << dataLen << std::endl; 169 | if (!m_bRecordData || (m_bRecordData && m_bShowImagesEnabled)) 170 | m_pDataProcessor->processMIPIData(m_pMipiPackage, dataLen, time_stamp_end, imu_data); 171 | } 172 | } 173 | } 174 | } 175 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/kalibr/sync_images.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | 22 | #include "sync_images.h" 23 | 24 | 25 | SyncImages::SyncImages(const ros::NodeHandle &nh) 26 | : nh_(nh), 27 | store_dir_("./"), 28 | save_(false) 29 | { 30 | 31 | /* 32 | * Create a new clean directory to store images 33 | */ 34 | nh_.param("store_dir", store_dir_, store_dir_); 35 | // TODO Fix bugs, doesn't work. 36 | // if (!boost::filesystem::is_empty(store_dir_)) { 37 | // boost::filesystem::remove_all(store_dir_); 38 | // } 39 | // if (!boost::filesystem::exists(store_dir_)) { 40 | // boost::filesystem::create_directory(store_dir_); 41 | // boost::filesystem::create_directory(store_dir_ + "frame/"); 42 | // boost::filesystem::create_directory(store_dir_ + "events/"); 43 | // } 44 | 45 | /* 46 | * Create sync images handle 47 | */ 48 | std::string frame_topic_name("/frame_image"); 49 | nh_.param("frame_topic", frame_topic_name, frame_topic_name); 50 | p_frame_sync_sub_ = std::make_shared(nh_, frame_topic_name, 2, 51 | ros::TransportHints().tcpNoDelay()); 52 | std::string events_topic_name("/events_image"); 53 | nh_.param("events_topic", events_topic_name, events_topic_name); 54 | p_events_sync_sub_ = std::make_shared(nh_, events_topic_name, 2, 55 | ros::TransportHints().tcpNoDelay()); 56 | 57 | p_sync_ = std::make_shared>( 58 | SyncPolicy(10), *p_frame_sync_sub_, *p_events_sync_sub_ 59 | ); 60 | p_sync_->registerCallback(boost::bind(&SyncImages::SyncImagesCallback, this, _1, _2)); 61 | 62 | p_ddyn_rec_ = std::make_shared(nh_); 63 | p_ddyn_rec_->registerVariable("save", &save_, "Save images", false, true); 64 | p_ddyn_rec_->publishServicesTopics(); 65 | // frame_sub_ = nh_.subscribe(frame_topic_name, 10, 66 | // &SyncImages::FrameImageCallback, 67 | // this); 68 | // events_sub_ = nh_.subscribe(events_topic_name, 10, 69 | // &SyncImages::EventsImageCallback, 70 | // this); 71 | } 72 | 73 | 74 | SyncImages::~SyncImages() = default; 75 | 76 | 77 | void SyncImages::SyncImagesCallback(const sensor_msgs::ImageConstPtr &frame_msg, 78 | const sensor_msgs::ImageConstPtr &events_msg) 79 | { 80 | if (save_) 81 | { 82 | cv_bridge::CvImagePtr frame_image_ptr; 83 | cv_bridge::CvImagePtr events_image_ptr; 84 | try 85 | { 86 | // TODO Gray or RGB 87 | frame_image_ptr = cv_bridge::toCvCopy(frame_msg, sensor_msgs::image_encodings::BGR8); 88 | events_image_ptr = cv_bridge::toCvCopy(events_msg, sensor_msgs::image_encodings::MONO8); 89 | } 90 | catch (cv_bridge::Exception &e) 91 | { 92 | ROS_ERROR_STREAM("cv_bridge Exception:" << e.what()); 93 | return; 94 | } 95 | 96 | cv::Mat frame_img = frame_image_ptr->image; 97 | cv::Mat events_img = events_image_ptr->image; 98 | 99 | std::string pair_id = std::to_string(frame_msg->header.seq); 100 | // boost::filesystem::create_directory(store_dir_ + pair_id); 101 | 102 | // ROS_INFO("store_path, %s", store_dir.c_str()); 103 | cv::imwrite(store_dir_ + "frame/" + pair_id + ".jpg", frame_img); 104 | cv::imwrite(store_dir_ + "events/" + pair_id + ".jpg", events_img); 105 | cv::waitKey(1); 106 | ROS_INFO("Write! %s", pair_id.c_str()); 107 | } 108 | } 109 | 110 | // void SyncImages::FrameImageCallback(const sensor_msgs::ImageConstPtr &frame_msg) { 111 | // ROS_ERROR("Frame : %lf", frame_msg->header.stamp.toSec()); 112 | // } 113 | // 114 | // void SyncImages::EventsImageCallback(const sensor_msgs::ImageConstPtr &events_msg) { 115 | // ROS_WARN("Events: %lf", events_msg->header.stamp.toSec()); 116 | // } 117 | -------------------------------------------------------------------------------- /celex5_ros/src/bean/celex5_ros_bean.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #include "celex5_ros_bean.h" 21 | 22 | using namespace celex5_ros; 23 | 24 | celex5_ros::CeleX5ROSBean::CeleX5ROSBean(const ros::NodeHandle &nh) 25 | : nh_(nh) { 26 | 27 | p_celex5_options_ = CeleX5Options::GetInstance(); 28 | /* 29 | * Read initial parameters from ROS Param Server 30 | */ 31 | ReadParams(); 32 | 33 | p_celex5_sensor_ = std::make_shared(); 34 | p_celex5_configure_ = 35 | std::make_shared(p_celex5_sensor_, nh_); 36 | } 37 | 38 | celex5_ros::CeleX5ROSBean::~CeleX5ROSBean() = default; 39 | 40 | void celex5_ros::CeleX5ROSBean::Run() { 41 | /* 42 | * Open CeleX5 Sensor 43 | */ 44 | std::string sensor_cfg_file_dir; 45 | CeleX5DdyConfigure::ReadROSParam(nh_, "sensor_cfg_file_dir", sensor_cfg_file_dir); 46 | p_celex5_sensor_->setSensorCfgFileDir(sensor_cfg_file_dir); 47 | std::string fpn_file_dir; 48 | CeleX5DdyConfigure::ReadROSParam(nh_, "fpn_file_dir", fpn_file_dir); 49 | p_celex5_sensor_->setFpnFileDir(fpn_file_dir); 50 | 51 | /* 52 | * Open sensor 53 | */ 54 | int device_type = CeleX5::DeviceType::Unknown_Devive; 55 | CeleX5DdyConfigure::ReadROSParam(nh_, "device_type", device_type); 56 | p_celex5_sensor_->openSensor(static_cast(device_type)); 57 | ROS_INFO("Sensor status: %d", p_celex5_sensor_->isSensorReady()); 58 | 59 | /* 60 | * Update CeleX5 sensor config with default value 61 | */ 62 | p_celex5_configure_->UpdateCeleX5AllOptions(); 63 | 64 | /* 65 | * Create CeleX5DataForwarder handle to forward full stream data from CeleX5 Sensor to ROS framework 66 | */ 67 | p_celex5_data_forwarder_ = std::make_shared(nh_, p_celex5_sensor_); 68 | 69 | /* 70 | * Provide display function. Can change the display fps in rqt_reconfigure 71 | */ 72 | p_celex5_display_ = CeleX5DisplayController::GetInstance(nh_, 73 | p_celex5_sensor_); 74 | p_celex5_display_->SetCeleX5Mode(p_celex5_sensor_->getSensorFixedMode()); 75 | 76 | } 77 | 78 | void celex5_ros::CeleX5ROSBean::ReadParams() { 79 | /* 80 | * Read parameters from ROS param server 81 | */ 82 | 83 | // TODO Get default value from sensor 84 | int tmp_celex5_mode_param = p_celex5_options_->GetFixedMode(); 85 | CeleX5DdyConfigure::ReadROSParam(nh_, "fixed_mode", tmp_celex5_mode_param); 86 | p_celex5_options_->SetFixedMode(static_cast(tmp_celex5_mode_param)); 87 | CeleX5DdyConfigure::ReadROSParam(nh_, "loop_mode1", tmp_celex5_mode_param); 88 | p_celex5_options_->SetLoopMode1(static_cast(tmp_celex5_mode_param)); 89 | CeleX5DdyConfigure::ReadROSParam(nh_, "loop_mode2", tmp_celex5_mode_param); 90 | p_celex5_options_->SetLoopMode2(static_cast(tmp_celex5_mode_param)); 91 | CeleX5DdyConfigure::ReadROSParam(nh_, "loop_mode3", tmp_celex5_mode_param); 92 | p_celex5_options_->SetLoopMode3(static_cast(tmp_celex5_mode_param)); 93 | 94 | CeleX5DdyConfigure::ReadROSParam(nh_, "event_frame_time", p_celex5_options_->event_frame_time_); 95 | CeleX5DdyConfigure::ReadROSParam(nh_, "optical_flow_frame_time", p_celex5_options_->optical_flow_frame_time_); 96 | CeleX5DdyConfigure::ReadROSParam(nh_, "threshold", p_celex5_options_->threshold_); 97 | CeleX5DdyConfigure::ReadROSParam(nh_, "brightness", p_celex5_options_->brightness_); 98 | CeleX5DdyConfigure::ReadROSParam(nh_, "imu_enabled", p_celex5_options_->imu_enabled_); 99 | CeleX5DdyConfigure::ReadROSParam(nh_, "ISO_level", p_celex5_options_->ISO_level_); 100 | // readIntParam("contrast", p_celex5_options_->contrast_); 101 | CeleX5DdyConfigure::ReadROSParam(nh_, "clock_rate", p_celex5_options_->clock_rate_); 102 | CeleX5DdyConfigure::ReadROSParam(nh_, "is_loop_mode_enabled", p_celex5_options_->is_loop_mode_enabled_); 103 | CeleX5DdyConfigure::ReadROSParam(nh_, "event_duration_in_loop", p_celex5_options_->event_duration_in_loop_); 104 | CeleX5DdyConfigure::ReadROSParam(nh_, "picture_number_in_loop", p_celex5_options_->picture_number_in_loop_); 105 | CeleX5DdyConfigure::ReadROSParam(nh_, "event_FPN_file_path", p_celex5_options_->event_FPN_file_path_); 106 | CeleX5DdyConfigure::ReadROSParam(nh_, "frame_FPN_file_path", p_celex5_options_->frame_FPN_file_path_); 107 | 108 | } 109 | 110 | -------------------------------------------------------------------------------- /celex5_calibration/src/intrinsics_extrinsics/pkg_camera_calibration/README.md: -------------------------------------------------------------------------------- 1 | *[中文版本](README_zh.md)* 2 | 3 | # CeleX5-MIPI Intrinsics Calibration Demo 4 | 5 | > Calibrate CeleX5-MIPI camera **intrinsics** with `camera-calibration`. 6 | 7 | - [Calibration principle](#calibration-principle) 8 | - [Running](#running) 9 | - [Example results](#example-results) 10 | 11 | We use the blinking chessboard calibration board (generated with [pattern-generator](../pattern/) in this repository) to calibrate the CeleX™ CeleX5-MIPI Event-based Camera. 12 | 13 | ## **Calibration principle** 14 | 15 | By using the blinking chessboard, the CeleX5-MIPI camera can generate a chessboard like event frame, and then the traditional Camera Intrinsics calibration method can be used. Here [`camera calibration`](http://wiki.ros.org/camera'calibration/tutorials/monocularcalibration) calibration tool integrated in ROS is used for calibration, and Zhang Zhengyou-Calibration method is used internally. The calibration results are saved with .yaml file, .txt file and image frame for final calibration results. 16 | 17 | ## Running 18 | 19 | 1. First install `camera-calibration`. 20 | 21 | ```bash 22 | sudo apt install ros-kinetic-camera-calibration 23 | ``` 24 | 25 | 2. Run `celex5_ros_node`: 26 | 27 | ```bash 28 | roslaunch celex5_ros celex5_ros_node.launch 29 | ``` 30 | 31 | Open `rqt_reconfigure` panel, under **Event Off-Pixel Timestamp Mode** sensor mode, only keep `/celex5_mipi/display/binary_img` Image topic publishing messages, close other Image topics to reduce running consumption. Keep `/celex5_mipi/display/binary_img` publishing frequency at 20 fps, too high can cause a delay in the calibration procedure. 32 | 33 | 3. Launch `camera-calibration`. For the detailed meaning and more usage of each parameter, please go to the official document of `camera-calibration` in the above link. 34 | 35 | ```bash 36 | rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.0212 image:=/celex5_mipi/display/binary_img 37 | ``` 38 | 39 | The meaning of parameters: 40 | 41 | - `--size`: The number of inner points (that is, the number of squares in the outer circle with the longest side out - 1), width*height. If this parameter is not set correctly, the calibration procedure will be found to be unresponsive in the following calibration steps. 42 | - `--square`: the physical length of each side of the squares in the calibration board. The unit is m. 43 | - `image:=`: Image topic name. 44 | 45 | After successful operation, the following windows will appear: 46 | 47 | 48 | 49 | Move the blinking chessboard screen, and make the X, Y, Size and Skew progress bars in the upper right corner full and green as much as possible. 50 | 51 | Reference the [official document of `camera-calibration`](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration): 52 | 53 | - checkerboard on the camera's left, right, top and bottom of field of view 54 | - X bar - left/right in field of view 55 | - Y bar - top/bottom in field of view 56 | - Size bar - toward/away and tilt from the camera 57 | - checkerboard filling the whole field of view 58 | - checkerboard tilted to the left, right, top and bottom (Skew) 59 | 60 | 61 | *Tips: In the process of moving, the blinking chessboard pattern can be suspended, so as to avoid corner detection when the dragging is serious. After fixing the position, continue to make the checkerboard blinking to obtain a high-quality Event frame* 62 | 63 | When you see the **CALIBRATE** button turns green, click calculate to get the internal parameter calibration result. Then click the **SAVE** button to save the calibration results. It will prompt you to save the path, which is a compressed package. After decompression, the image frame used in the calibration process and calibration result file (one .yaml file and one .txt file) are stored. 64 | 65 | ## Example results 66 | 67 | This is the whole calibration output: 68 | 69 | 70 | 71 | ost.yaml: 72 | 73 | ```yaml 74 | image_width: 1280 75 | image_height: 800 76 | camera_name: narrow_stereo 77 | camera_matrix: 78 | rows: 3 79 | cols: 3 80 | data: [1687.514919, 0.000000, 676.131784, 0.000000, 1694.799737, 417.348889, 0.000000, 0.000000, 1.000000] 81 | distortion_model: plumb_bob 82 | distortion_coefficients: 83 | rows: 1 84 | cols: 5 85 | data: [-0.089225, -0.433622, 0.003022, -0.001306, 0.000000] 86 | rectification_matrix: 87 | rows: 3 88 | cols: 3 89 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 90 | projection_matrix: 91 | rows: 3 92 | cols: 4 93 | data: [1647.456299, 0.000000, 677.701102, 0.000000, 0.000000, 1683.463867, 418.507924, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 94 | ``` 95 | 96 | The sample image frame used in the calibration: 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /celex5_ros/launch/includes/nodelet.launch.xml: -------------------------------------------------------------------------------- 1 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 74 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /celex5_ros/src/bean/celex5_options.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The ROS package for CeleX^TM CeleX5-MIPI Dynamic Vision Sensor. 3 | * 4 | * Copyright (C) 2020 Kehan.Xue 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef CELEX5_ROS_SRC_BEAN_CELEX5_OPTIONS_H_ 21 | #define CELEX5_ROS_SRC_BEAN_CELEX5_OPTIONS_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include "celex5/celex5.h" 28 | 29 | namespace celex5_ros { 30 | 31 | class CeleX5Options { 32 | 33 | private: 34 | // TODO Default value is get from sensor 35 | explicit CeleX5Options(CeleX5::CeleX5Mode fixed_mode = CeleX5::CeleX5Mode::Event_Off_Pixel_Timestamp_Mode, 36 | bool is_loop_mode_enabled = false, 37 | std::vector loop_modes = std::vector(3), 38 | uint32_t event_duration_in_loop = 100, 39 | uint32_t picture_number_in_loop = 30, 40 | uint32_t event_frame_time_ = 30000, 41 | uint32_t optical_flow_frame_time_ = 20, 42 | uint32_t threshold = 185, 43 | uint32_t brightness = 150, 44 | uint32_t ISO_level = 2, 45 | bool raw_events_enabled = true, 46 | bool polarity_img_enabled = true, 47 | bool imu_enabled = true, 48 | // uint32_t contrast = 1, 49 | uint32_t clock_rate = 100, 50 | std::string event_fpn_file_path = "", 51 | std::string frame_fpn_file_path = ""); 52 | 53 | CeleX5Options(const CeleX5Options &); 54 | CeleX5Options &operator=(const CeleX5Options &); 55 | static std::shared_ptr instance; 56 | static std::shared_ptr mutex_instance; 57 | 58 | public: 59 | 60 | static std::shared_ptr GetInstance(); 61 | 62 | virtual ~CeleX5Options(); 63 | 64 | CeleX5::CeleX5Mode GetFixedMode() const; 65 | void SetFixedMode(CeleX5::CeleX5Mode fixed_mode); 66 | 67 | bool IsLoopModeEnabled() const; 68 | void SetIsLoopModeEnabled(bool is_loop_mode_enabled); 69 | 70 | const std::vector &GetLoopModes() const; 71 | void SetLoopModes(const std::vector &loop_modes); 72 | void SetLoopMode1(CeleX5::CeleX5Mode loop_mode1); 73 | void SetLoopMode2(CeleX5::CeleX5Mode loop_mode2); 74 | void SetLoopMode3(CeleX5::CeleX5Mode loop_mode3); 75 | 76 | uint32_t GetEventFrameTime() const; 77 | void SetEventFrameTime(uint32_t event_frame_time); 78 | 79 | uint32_t GetOpticalFlowFrameTime() const; 80 | void SetOpticalFlowFrameTime(uint32_t optical_flow_frame_time); 81 | 82 | uint32_t GetThreshold() const; 83 | void SetThreshold(uint32_t threshold); 84 | 85 | uint32_t GetBrightness() const; 86 | void SetBrightness(uint32_t brightness); 87 | 88 | // uint32_t GetContrast() const; 89 | // void SetContrast(uint32_t contrast); 90 | 91 | uint32_t GetClockRate() const; 92 | void SetClockRate(uint32_t clock_rate); 93 | 94 | uint32_t GetEventDurationInLoop() const; 95 | void SetEventDurationInLoop(uint32_t event_duration); 96 | 97 | uint32_t GetPictureNumberInLoop() const; 98 | void SetPictureNumberInLoop(uint32_t picture_number_in_loop); 99 | 100 | const std::string &GetEventFpnFilePath() const; 101 | void SetEventFpnFilePath(const std::string &event_fpn_file_path); 102 | 103 | const std::string &GetFrameFpnFilePath() const; 104 | void SetFrameFpnFilePath(const std::string &frame_fpn_file_path); 105 | 106 | bool IsImuEnabled() const; 107 | void SetImuEnabled(bool imu_enabled); 108 | 109 | uint32_t GetIsoLevel() const; 110 | void SetIsoLevel(uint32_t iso_level); 111 | 112 | bool IsRawEventsEnabled() const; 113 | void SetRawEventsEnabled(bool raw_events_enabled); 114 | 115 | bool IsPolarityImgEnabled() const; 116 | void SetPolarityImgEnabled(bool polarity_img_enabled); 117 | 118 | // private: 119 | bool raw_events_enabled_; 120 | bool polarity_img_enabled_; 121 | bool imu_enabled_; 122 | 123 | CeleX5::CeleX5Mode fixed_mode_; 124 | std::vector loop_modes_; 125 | 126 | uint32_t event_frame_time_; 127 | uint32_t optical_flow_frame_time_; 128 | 129 | uint32_t threshold_; 130 | uint32_t brightness_; 131 | uint32_t ISO_level_; 132 | // uint32_t contrast_; 133 | uint32_t clock_rate_; 134 | 135 | bool is_loop_mode_enabled_; 136 | uint32_t event_duration_in_loop_; 137 | uint32_t picture_number_in_loop_; 138 | 139 | std::string event_FPN_file_path_; 140 | std::string frame_FPN_file_path_; 141 | }; 142 | } 143 | 144 | #endif //CELEX5_ROS_SRC_BEAN_CELEX5_OPTIONS_H_ 145 | -------------------------------------------------------------------------------- /celex5_calibration/src/temporal/temporal_offset_calculate.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The celex5_calibration ROS package, 3 | * used to calibrate camera parameters for CeleX5-MIPI Event-based Camera. 4 | * 5 | * Copyright (C) 2020 Kehan.Xue 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | */ 20 | 21 | #include "temporal_offset_calculate.h" 22 | 23 | // TemporalOffsetCalculate::TemporalOffsetCalculate(const ros::NodeHandle &nh, bool show_match) 24 | // : nh_(nh), 25 | // show_match_(show_match), 26 | // corners_col_num_(8), // default value 27 | // corners_row_num_(6) { 28 | // nh_.param("corners_col_num", corners_col_num_, corners_col_num_); 29 | // nh_.param("corners_row_num", corners_row_num_, corners_row_num_); 30 | // 31 | // p_ddyn_rec_ = std::make_shared(nh_); 32 | // p_ddyn_rec_->registerVariable("show_match", &show_match_, "Show corners matching"); 33 | // p_ddyn_rec_->publishServicesTopics(); 34 | // } 35 | // 36 | // TemporalOffsetCalculate::~TemporalOffsetCalculate() { 37 | // 38 | // } 39 | 40 | 41 | TemporalOffsetCalculate::TemporalOffsetCalculate(const ros::NodeHandle &nh) 42 | : nh_(nh), 43 | is_plot_(true), 44 | x_length_(20) 45 | { 46 | 47 | init_stamp_ = ros::Time::now(); 48 | last_events_stamp_ = init_stamp_; 49 | usleep(3000); 50 | 51 | std::string events_topic("/events"); 52 | nh_.param("events_topic", events_topic, events_topic); 53 | events_sub_ = nh_.subscribe(events_topic, 1, 54 | &TemporalOffsetCalculate::EventsCallback, 55 | this); 56 | std::string frame_topic("/frame"); 57 | nh_.param("frame_topic", frame_topic, frame_topic); 58 | frame_sub_ = nh_.subscribe(frame_topic, 1, 59 | &TemporalOffsetCalculate::FrameCallback, 60 | this); 61 | 62 | p_ddyn_rec_ = std::make_shared(nh_); 63 | p_ddyn_rec_->registerVariable("is_plot", &is_plot_, "Whether plot.", false, true); 64 | p_ddyn_rec_->registerVariable("x_length", &x_length_, "X axis range in plot.", -2000, 2000); 65 | p_ddyn_rec_->publishServicesTopics(); 66 | } 67 | 68 | 69 | TemporalOffsetCalculate::~TemporalOffsetCalculate() 70 | { 71 | 72 | } 73 | 74 | 75 | void TemporalOffsetCalculate::EventsCallback(const celex5_msgs::EventVectorConstPtr &msg) 76 | { 77 | // ROS_WARN("Get callback!"); 78 | vec_events_rate_stamps_.emplace_back((msg->header.stamp - init_stamp_).toSec()); 79 | ros::Duration duration = msg->header.stamp - last_events_stamp_; 80 | double events_rate = std::log((msg->events.size() / duration.toSec())); 81 | vec_events_rate_history_.emplace_back(events_rate); 82 | AnimationPlot(); 83 | last_events_stamp_ = msg->header.stamp; 84 | } 85 | 86 | 87 | void TemporalOffsetCalculate::FrameCallback(const sensor_msgs::ImageConstPtr &msg) 88 | { 89 | cv::Mat current_frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // TODO encoding 90 | cv::cvtColor(current_frame, current_frame, cv::COLOR_RGB2GRAY); 91 | if (last_frame_.empty()) 92 | { 93 | // Didn't initial 94 | last_frame_ = current_frame; 95 | return; 96 | } 97 | else 98 | { 99 | int64_t intensity_changes = std::log(CalculateIntensityChanges(last_frame_, current_frame)); 100 | vec_intensity_changes_stamps_.emplace_back((msg->header.stamp - init_stamp_).toSec()); 101 | vec_intensity_changes_history_.emplace_back(intensity_changes); 102 | AnimationPlot(); 103 | last_frame_ = current_frame; 104 | } 105 | } 106 | 107 | 108 | int64_t TemporalOffsetCalculate::CalculateIntensityChanges(const cv::Mat &frame1, const cv::Mat &frame2) 109 | { 110 | int64_t intensity = 0; 111 | if (frame1.rows != frame2.rows || frame1.cols != frame2.cols) 112 | { 113 | return -1; 114 | } 115 | for (int i = 0; i < frame2.rows; ++i) 116 | { 117 | for (int j = 0; j < frame2.cols; ++j) 118 | { 119 | intensity += static_cast(std::abs(frame2.at(i, j) - frame1.at(i, j))); 120 | } 121 | } 122 | return intensity; 123 | } 124 | 125 | 126 | void TemporalOffsetCalculate::AnimationPlot() 127 | { 128 | // ROS_WARN("Get plot!"); 129 | plt::clf();; 130 | plt::named_plot("Events Rate", vec_events_rate_stamps_, vec_events_rate_history_); 131 | plt::named_plot("Intensity Changes", vec_intensity_changes_stamps_, vec_intensity_changes_history_); 132 | double now_stamped = vec_events_rate_stamps_.at(vec_events_rate_stamps_.size() - 1); 133 | plt::xlim((now_stamped - 20 > 0 ? (now_stamped - x_length_) : 0), now_stamped); 134 | plt::title("Temporal Offset"); 135 | plt::legend(); 136 | plt::pause(0.001); 137 | } 138 | 139 | 140 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/include/celextypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All Rights Reserved 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CELEXTYPES_H 18 | #define CELEXTYPES_H 19 | 20 | #include 21 | #include 22 | 23 | #define SLIDER_DELAY 100 24 | #define MAX_LOG_LINES 100 25 | 26 | #define MAX_PAGE_COUNT 100000 27 | 28 | #define PIPEOUT_TIMER 10 29 | #define EVENT_SIZE 4 30 | #define PIXELS_PER_COL 768 31 | #define PIXELS_PER_ROW 640 32 | #define PIXELS_NUMBER 491520 33 | 34 | #define CELEX5_COL 1280 35 | #define CELEX5_ROW 800 36 | #define CELEX5_PIXELS_NUMBER 1024000 37 | 38 | #define MIRROR_VERTICAL 1 39 | #define MIRROR_HORIZONTAL 1 40 | 41 | #define FILE_CELEX5_CFG "CeleX5_Commands_MIPI.xml" 42 | #define FILE_CELEX5_CFG_MIPI "cfg_mp" 43 | #define FILE_CELEX5_CFG_MIPI_WRIE "cfg_mp_wire" 44 | 45 | #define SEQUENCE_LAYOUT_WIDTH 3 //7 46 | #define SLIDER_LAYOUT_WIDTH 1 //4 47 | #define DIALOG_LAYOUT_WIDTH 2 48 | 49 | #define FPN_CALCULATION_TIMES 5 50 | 51 | #define BIAS_RAMPP_H 16 52 | #define BIAS_RAMPP_L 17 53 | #define BIAS_BRT_I_H 22 54 | #define BIAS_BRT_I_L 23 55 | #define BIAS_ADVH_I_H 26 56 | #define BIAS_ADVH_I_L 27 57 | #define BIAS_ADVL_I_H 30 58 | #define BIAS_ADVL_I_L 31 59 | #define BIAS_ADVCH_I_H 34 60 | #define BIAS_ADVCH_I_L 35 61 | #define BIAS_ADVCL_I_H 38 62 | #define BIAS_ADVCL_I_L 39 63 | 64 | #define BIAS_VCM_I_H 42 65 | #define BIAS_VCM_I_L 43 66 | 67 | #define ROW_ENABLE 44 68 | #define COL_GAIN 45 69 | 70 | #define SENSOR_MODE_1 53 71 | #define SENSOR_MODE_2 54 72 | #define SENSOR_MODE_3 55 73 | 74 | #define EVENT_DURATION_H 58 75 | #define EVENT_DURATION_L 57 76 | 77 | #define PICTURE_NUMBER_1 59 78 | #define PICTURE_NUMBER_2 60 79 | #define PICTURE_NUMBER_3 61 80 | #define PICTURE_NUMBER_4 62 81 | #define PICTURE_NUMBER_5 63 82 | 83 | #define SENSOR_MODE_SELECT 64 84 | 85 | #define EVENT_PACKET_SELECT 73 86 | 87 | #define MIPI_ROW_NUM_EVENT_H 79 88 | #define MIPI_ROW_NUM_EVENT_L 80 89 | 90 | #define MIPI_HD_GAP_FULLFRAME_H 82 91 | #define MIPI_HD_GAP_FULLFRAME_L 83 92 | 93 | #define MIPI_HD_GAP_EVENT_H 84 94 | #define MIPI_HD_GAP_EVENT_L 85 95 | 96 | #define MIPI_GAP_EOF_SOF_H 86 97 | #define MIPI_GAP_EOF_SOF_L 87 98 | 99 | #define SOFT_RESET 90 100 | #define SOFT_TRIGGER 93 101 | #define PADDR_EN 94 102 | 103 | #define MIPI_PLL_DIV_I 113 104 | #define MIPI_PLL_DIV_N_H 115 105 | #define MIPI_PLL_DIV_N_L 114 106 | 107 | #define MIPI_NPOWD_PLL 139 108 | #define MIPI_NPOWD_BGR 140 109 | #define MIPI_NRSET_PLL 141 110 | #define MIPI_NPOWD_PHY 142 111 | #define MIPI_NRSET_PHY 143 112 | #define MIPI_NDIS_PHY 144 113 | 114 | #define PLL_PD_B 150 115 | #define PLL_FOUT_DIV1 151 116 | #define PLL_FOUT_DIV2 152 117 | #define PLL_DIV_N 159 118 | #define PLL_DIV_L 160 119 | #define PLL_FOUT3_POST_DIV 169 120 | 121 | #define FLICKER_DETECT_EN 183 122 | 123 | #define AUTOISP_PROFILE_ADDR 220 124 | #define AUTOISP_BRT_EN 221 125 | #define AUTOISP_TEM_EN 222 126 | #define AUTOISP_TRIGGER 223 127 | 128 | #define AUTOISP_BRT_VALUE_H 233 129 | #define AUTOISP_BRT_VALUE_L 232 130 | 131 | #define AUTOISP_BRT_THRES1_H 235 132 | #define AUTOISP_BRT_THRES1_L 234 133 | 134 | #define AUTOISP_BRT_THRES2_H 237 135 | #define AUTOISP_BRT_THRES2_L 236 136 | 137 | #define AUTOISP_BRT_THRES3_H 239 138 | #define AUTOISP_BRT_THRES3_L 238 139 | 140 | #define VIRTUAL_USB_ADDR 254 141 | 142 | typedef struct EventData 143 | { 144 | uint16_t col; 145 | uint16_t row; 146 | uint16_t adc; //Event_Off_Pixel_Timestamp_Mode: adc is 0; Event Intensity Mode: adc is "Intensity"; Event_In_Pixel_Timestamp_Mode: adc is "Optical-flow T" 147 | int16_t polarity; //-1: intensity weakened; 1: intensity is increased; 0 intensity unchanged 148 | uint32_t tInPixelIncreasing; 149 | uint32_t tOffPixelIncreasing; //it won't be reset, it's a monotonically increasing value 150 | } EventData; 151 | 152 | typedef enum EventShowType 153 | { 154 | EventShowByTime = 0, 155 | EventShowByCount = 1, 156 | EventShowByRowCycle = 2, 157 | } EventShowType; 158 | 159 | typedef enum PlaybackState { 160 | NoBinPlaying = 0, 161 | Playing, 162 | BinReadFinished, 163 | PlayFinished, 164 | Replay 165 | } PlaybackState; 166 | 167 | typedef struct IMUData { 168 | double xGYROS; 169 | double yGYROS; 170 | double zGYROS; 171 | double xACC; 172 | double yACC; 173 | double zACC; 174 | double xMAG; 175 | double yMAG; 176 | double zMAG; 177 | double xTEMP; 178 | uint64_t frameNo; 179 | std::time_t timestamp; 180 | } IMUData; 181 | 182 | typedef struct IMURawData 183 | { 184 | uint8_t imuData[20]; 185 | std::time_t timestamp; 186 | } IMURawData; 187 | 188 | #endif // CELEXTYPES_H 189 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/eventproc/datarecorder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "datarecorder.h" 18 | 19 | #ifdef _WIN32 20 | #include 21 | CRITICAL_SECTION g_csDataRecorder; 22 | #else 23 | #endif 24 | 25 | #define TIMESTAMP_LENGTH 8 26 | #define PACKAGE_SIZE_LENGTH 4 27 | #define IMU_SIZE_LENGTH 4 28 | #define MAX_PACKAGE_SIZE 1536001 29 | 30 | DataRecorder::DataRecorder() 31 | : m_bRecording(false) 32 | , m_iTimeStampStart(0) 33 | , m_uiPackageCount(0) 34 | { 35 | #ifdef _WIN32 36 | InitializeCriticalSection(&g_csDataRecorder); 37 | #else 38 | #endif 39 | } 40 | 41 | DataRecorder::~DataRecorder() 42 | { 43 | } 44 | 45 | /* 46 | * @function: startRecording 47 | * @brief : start recording raw data of CeleX-5 sensors 48 | * @input : filePath: bin file path and name to record 49 | * @output : 50 | * @return : true is successful to start recording 51 | */ 52 | bool DataRecorder::startRecording(std::string filePath) 53 | { 54 | //cout << __FUNCTION__ << ": start open file!" << endl; 55 | m_iTimeStampStart = getLocalTimestamp(); 56 | 57 | m_ofstreamRecord.open(filePath.c_str(), std::ios::binary); 58 | if (!m_ofstreamRecord.is_open()) 59 | { 60 | std::cout << __FUNCTION__ << "Can't open recording file." << std::endl; 61 | return false; 62 | } 63 | std::cout << __FUNCTION__ << ": filePath = " << filePath << std::endl; 64 | 65 | CeleX5::BinFileAttributes header; 66 | m_ofstreamRecord.write((char*)&header, sizeof(CeleX5::BinFileAttributes)); 67 | 68 | m_bRecording = true; 69 | m_uiPackageCount = 0; 70 | 71 | return true; 72 | } 73 | 74 | /* 75 | * @function: stopRecording 76 | * @brief : stop recording raw data of CeleX-5 sensors 77 | * @input : header: the header of bin file 78 | * @output : 79 | * @return : 80 | */ 81 | void DataRecorder::stopRecording(CeleX5::BinFileAttributes* header) 82 | { 83 | //cout << __FUNCTION__ << ": close file!" << endl; 84 | #ifdef _WIN32 85 | EnterCriticalSection(&g_csDataRecorder); 86 | #endif 87 | 88 | m_bRecording = false; 89 | 90 | int iTimeRecorded = getLocalTimestamp() - m_iTimeStampStart; 91 | 92 | int hour = iTimeRecorded / 3600; 93 | int minute = (iTimeRecorded % 3600) / 60; 94 | int second = (iTimeRecorded % 3600) % 60; 95 | 96 | // write a header 97 | header->hour = hour; 98 | header->minute = minute; 99 | header->second = second; 100 | header->packageCount = m_uiPackageCount; 101 | 102 | m_ofstreamRecord.seekp(0, std::ios::beg); 103 | m_ofstreamRecord.write((char*)header, sizeof(CeleX5::BinFileAttributes)); 104 | m_ofstreamRecord.flush(); 105 | m_ofstreamRecord.close(); 106 | 107 | #ifdef _WIN32 108 | LeaveCriticalSection(&g_csDataRecorder); 109 | #endif 110 | } 111 | 112 | /* 113 | * @function: writeData 114 | * @brief : wirte vecData (sensor data) into bin file 115 | * @input : vecData: data vector to be written 116 | * @output : 117 | * @return : true 118 | */ 119 | bool DataRecorder::writeData(uint8_t* pData, uint32_t length) 120 | { 121 | if (!m_ofstreamRecord.is_open()) 122 | return false; 123 | uint32_t size = length; 124 | if (length > MAX_PACKAGE_SIZE) 125 | return false; 126 | 127 | m_uiPackageCount++; 128 | 129 | //--- write package size --- 130 | m_ofstreamRecord.write((char*)&size, PACKAGE_SIZE_LENGTH); 131 | 132 | //--- write package data --- 133 | m_ofstreamRecord.write((char*)pData, size); 134 | 135 | return true; 136 | } 137 | 138 | /* 139 | * @function: writeData 140 | * @brief : wirte vecData (sensor data), sensor data timestamp and imuData into bin file 141 | * @input : vecData: data vector to be written 142 | * time_stamp_end: the pc timestamp when received the vecData (sensor data) 143 | * imuData: a vector of imu raw data 144 | * @output : 145 | * @return : rue is successful to write data 146 | */ 147 | bool DataRecorder::writeData(uint8_t* pData, uint32_t length, std::time_t time_stamp_end, std::vector imuData) 148 | { 149 | #ifdef _WIN32 150 | EnterCriticalSection(&g_csDataRecorder); 151 | #endif 152 | 153 | if (!m_ofstreamRecord.is_open()) 154 | { 155 | #ifdef _WIN32 156 | LeaveCriticalSection(&g_csDataRecorder); 157 | #endif 158 | return false; 159 | } 160 | 161 | if (!writeData(pData, length)) //write the data buffer 162 | { 163 | #ifdef _WIN32 164 | LeaveCriticalSection(&g_csDataRecorder); 165 | #endif 166 | return false; 167 | } 168 | m_ofstreamRecord.write((char*)&time_stamp_end, TIMESTAMP_LENGTH); //timestamp of frame 169 | // write the imu data 170 | uint32_t size = imuData.size(); 171 | m_ofstreamRecord.write((char*)&size, IMU_SIZE_LENGTH); 172 | m_ofstreamRecord.write((char*)imuData.data(), imuData.size()*sizeof(IMURawData)); 173 | 174 | #ifdef _WIN32 175 | LeaveCriticalSection(&g_csDataRecorder); 176 | #endif 177 | 178 | return true; 179 | } 180 | 181 | /* 182 | * @function: getLocalTime 183 | * @brief : get local time and convert it into an integer 184 | * @input : 185 | * @output : 186 | * @return : an integer type of local time 187 | */ 188 | int DataRecorder::getLocalTimestamp() 189 | { 190 | #ifdef _WIN32 191 | time_t ttCurrentT = time(NULL); 192 | tm stTm; 193 | localtime_s(&stTm, &ttCurrentT); 194 | 195 | return 3600 * stTm.tm_hour + 60 * stTm.tm_min + stTm.tm_sec; 196 | #else 197 | time_t ttCurrentT = time(NULL); 198 | struct tm* pTm; 199 | pTm = localtime(&ttCurrentT); 200 | return 3600 * pTm->tm_hour + 60 * pTm->tm_min + pTm->tm_sec; 201 | #endif 202 | } -------------------------------------------------------------------------------- /celex5_ros/src/sdk/base/dataqueue.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include "dataqueue.h" 20 | #include "../include/celextypes.h" 21 | 22 | DataQueue::DataQueue() 23 | : m_uiSize(0) 24 | , m_iHead(0) 25 | , m_iTail(0) 26 | { 27 | for (int i = 0; i < m_arrayData.size(); i++) 28 | { 29 | m_arrayData[i].length = MAX_DATA_LEN; 30 | m_arrayData[i].pData = new uint8_t[MAX_DATA_LEN]; 31 | } 32 | } 33 | 34 | DataQueue::~DataQueue() 35 | { 36 | for (int i = 0; i < m_arrayData.size(); i++) 37 | { 38 | delete[] m_arrayData[i].pData; 39 | m_arrayData[i].pData = nullptr; 40 | } 41 | } 42 | 43 | /* 44 | * @function: push 45 | * @brief : add a packet data at the end of the data array (m_arrayData) 46 | * @input : pData: a packet of sensor raw data 47 | * length: the size of the packet pData 48 | * timestamp: the pc timestamp when the packet pData was received 49 | * @output : 50 | * @return : 51 | */ 52 | bool DataQueue::push(uint8_t* pData, uint32_t length, std::time_t timestamp) 53 | { 54 | if (nullptr == pData) 55 | { 56 | return false; 57 | } 58 | if (m_uiSize == ARRAY_SIZE) 59 | { 60 | std::cout << __FUNCTION__ << ": data array is full!" << std::endl; 61 | return false; 62 | } 63 | int lenCopy = length > MAX_DATA_LEN ? MAX_DATA_LEN : length; 64 | memcpy(m_arrayData[m_iTail].pData, pData, lenCopy); 65 | m_arrayData[m_iTail].length = lenCopy; 66 | m_arrayData[m_iTail].timestamp = timestamp; 67 | m_uiSize++; 68 | m_iTail++; 69 | if (m_iTail == ARRAY_SIZE) 70 | m_iTail = 0; 71 | return true; 72 | } 73 | 74 | /* 75 | * @function: push 76 | * @brief : add a packet data at the end of the data array (m_arrayData) 77 | * @input : pData: a packet of sensor raw data 78 | * length: the size of the packet pData 79 | * imuData: a vector of imu raw data 80 | * timestamp: the pc timestamp when the packet pData was received 81 | * @output : 82 | * @return : 83 | */ 84 | bool DataQueue::push(uint8_t* pData, uint32_t length, std::vector &imuData, std::time_t timestamp) 85 | { 86 | if (nullptr == pData) 87 | { 88 | return false; 89 | } 90 | if (m_uiSize == ARRAY_SIZE) 91 | { 92 | std::cout << __FUNCTION__ << ": data array is full!" << std::endl; 93 | return false; 94 | } 95 | int lenCopy = length > MAX_DATA_LEN ? MAX_DATA_LEN : length; 96 | memcpy(m_arrayData[m_iTail].pData, pData, lenCopy); 97 | m_arrayData[m_iTail].length = lenCopy; 98 | m_arrayData[m_iTail].timestamp = timestamp; 99 | m_arrayData[m_iTail].vecIMUData = imuData; 100 | m_uiSize++; 101 | m_iTail++; 102 | if (m_iTail == ARRAY_SIZE) 103 | m_iTail = 0; 104 | return true; 105 | } 106 | 107 | /* 108 | * @function: pop 109 | * @brief : pop the head packet data in data array (m_arrayData) 110 | * @input : 111 | * @output : pData: a packet of sensor raw data 112 | * length: the size of the packet pData 113 | * timestamp: the pc timestamp when the packet pData was received 114 | * @return : 115 | */ 116 | bool DataQueue::pop(uint8_t* pData, uint32_t* length, std::time_t* timestamp) 117 | { 118 | if (nullptr == pData) 119 | { 120 | return false; 121 | } 122 | if (m_uiSize == 0) 123 | { 124 | std::cout << __FUNCTION__ << ": data array is empty!" << std::endl; 125 | return false; 126 | } 127 | if (nullptr == m_arrayData[m_iHead].pData) 128 | { 129 | //pData = NULL; 130 | *length = 0; 131 | } 132 | else 133 | { 134 | //pData = m_arrayData[m_iHead].pData; 135 | memcpy(pData, m_arrayData[m_iHead].pData, m_arrayData[m_iHead].length); 136 | *length = m_arrayData[m_iHead].length; 137 | *timestamp = m_arrayData[m_iHead].timestamp; 138 | } 139 | m_uiSize--; 140 | m_iHead++; 141 | if (ARRAY_SIZE == m_iHead) 142 | m_iHead = 0; 143 | return true; 144 | } 145 | 146 | /* 147 | * @function: pop 148 | * @brief : pop the head packet data in data array (m_arrayData) 149 | * @input : 150 | * @output : pData: a packet of sensor raw data 151 | * length: the size of the packet pData 152 | * imuData: a vector of imu raw data 153 | * timestamp: the pc timestamp when the packet pData was received 154 | * @return : 155 | */ 156 | bool DataQueue::pop(uint8_t* pData, uint32_t* length, std::vector& imuData, std::time_t* timestamp) 157 | { 158 | if (nullptr == pData) 159 | { 160 | return false; 161 | } 162 | if (m_uiSize == 0) 163 | { 164 | std::cout << __FUNCTION__ << ": data array is empty!" << std::endl; 165 | return false; 166 | } 167 | if (nullptr == m_arrayData[m_iHead].pData) 168 | { 169 | //pData = NULL; 170 | *length = 0; 171 | } 172 | else 173 | { 174 | //pData = m_arrayData[m_iHead].pData; 175 | memcpy(pData, m_arrayData[m_iHead].pData, m_arrayData[m_iHead].length); 176 | *length = m_arrayData[m_iHead].length; 177 | *timestamp = m_arrayData[m_iHead].timestamp; 178 | imuData.swap(m_arrayData[m_iHead].vecIMUData); 179 | } 180 | m_uiSize--; 181 | m_iHead++; 182 | if (ARRAY_SIZE == m_iHead) 183 | m_iHead = 0; 184 | return true; 185 | } 186 | 187 | /* 188 | * @function: size 189 | * @brief : get the size of the data array (m_arrayData) 190 | * @input : 191 | * @output : 192 | * @return : return the size of the data array (m_arrayData) 193 | */ 194 | uint32_t DataQueue::size() 195 | { 196 | return m_uiSize; 197 | } 198 | 199 | /* 200 | * @function: clear 201 | * @brief : reset the head / tail index, and data array (m_arrayData) size 202 | * @input : 203 | * @output : 204 | * @return : 205 | */ 206 | void DataQueue::clear() 207 | { 208 | m_uiSize = 0; 209 | m_iHead = 0; 210 | m_iTail = 0; 211 | } 212 | -------------------------------------------------------------------------------- /celex5_ros/src/sdk/cx3driver/cypress.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017-2020 CelePixel Technology Co. Ltd. All rights reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include "cypress.h" 19 | 20 | #define CYPRESS_DEVICE_VENDOR_ID 0x04b4 //0x2560 21 | #define CYPRESS_DEVICE_PRODUCT_ID 0x00f1 //0xd051 22 | 23 | #define ADDRESS_SERIAL_NUMBER 0x0001 24 | #define ADDRESS_FIRMWARE_VERSION 0x0002 25 | #define ADDRESS_FIRMWARE_DATE 0x0003 26 | 27 | Cypress::Cypress() 28 | { 29 | 30 | } 31 | 32 | Cypress::~Cypress() 33 | { 34 | 35 | } 36 | 37 | /* 38 | * @function: openUSB 39 | * @brief : open usb 40 | * @input : 41 | * @output : 42 | * @return : true is successful to open usb 43 | */ 44 | bool Cypress::openUSB(void) 45 | { 46 | if (usbOpen(CYPRESS_DEVICE_VENDOR_ID, CYPRESS_DEVICE_PRODUCT_ID, LIBUSB_TRANSFER_TYPE_BULK)) 47 | { 48 | return true; 49 | } 50 | usbClose(); 51 | return false; 52 | } 53 | 54 | /* 55 | * @function: openStream 56 | * @brief : open usb stream 57 | * @input : 58 | * @output : 59 | * @return : true is successful to open usb stream 60 | */ 61 | bool Cypress::openStream(void) 62 | { 63 | //开启视频流 64 | uint8_t video_start[] = { 0x00, 0x00,0x01,0x02,0x0A,0x8B,0x02,0x00, 0x00,0x00, 0x00,0x00, 0x00,0x00, 0x00,0x00, 0x00,0x00, 0x00,0x20,0x1C,0x00, 0x00,0x90,0x00, 0x00 }; 65 | //usb_control(0x41, 0x99, 0x00, 0x00, nullptr, 0); 66 | if (usbControl(0x21, 0x01, 0x201, 0x0, video_start, sizeof(video_start))) 67 | { 68 | if (start() == true) 69 | { 70 | return true; 71 | } 72 | usbControl(0x41, 0x88, 0x00, 0x00, nullptr, 0); 73 | } 74 | return false; 75 | } 76 | 77 | /* 78 | * @function: closeUSB 79 | * @brief : close usb 80 | * @input : 81 | * @output : 82 | * @return : 83 | */ 84 | void Cypress::closeUSB(void) 85 | { 86 | usbClose(); 87 | } 88 | 89 | /* 90 | * @function: closeStream 91 | * @brief : close usb stream 92 | * @input : 93 | * @output : 94 | * @return : 95 | */ 96 | void Cypress::closeStream(void) 97 | { 98 | usbControl(0x41, 0x88, 0x00, 0x00, nullptr, 0); 99 | stop(); 100 | } 101 | 102 | /* 103 | * @function: usbSet 104 | * @brief : 105 | * @input : wId: control type 106 | * reg: register address 107 | * value: register value 108 | * @output : 109 | * @return : true is successful to perform a USB control transfer 110 | */ 111 | bool Cypress::usbSet(uint16_t wId, uint16_t reg, uint16_t value) 112 | { 113 | unsigned char data[10]; 114 | 115 | *(uint16_t*)data = reg; 116 | *(uint16_t*)(data+2) = value; 117 | return usbControl(0x21, 0x1, wId, 0x300, data, 4); 118 | } 119 | 120 | /* 121 | * @function: usbGet 122 | * @brief : 123 | * @input : wId: control type 124 | * reg: register address 125 | * @output : value: register value 126 | * @return : true is successful to get data from USB control transfer 127 | */ 128 | bool Cypress::usbGet(uint16_t wId, uint16_t reg, uint16_t &value) 129 | { 130 | unsigned char data[10]; 131 | 132 | *(uint16_t*)data = reg; 133 | if (usbControl(0x21, 0x1, wId, 0x300, data, 4)) 134 | { 135 | if (usbControl(0xA1, 0x81, wId, 0x300, data, 4)) 136 | { 137 | value = *(uint16_t*)(data+2); 138 | return true; 139 | } 140 | } 141 | return false; 142 | } 143 | 144 | /* 145 | * @function: writeSerialNumber 146 | * @brief : write the serial number of sensor 147 | * @input : number: sensor serial number 148 | * @output : 149 | * @return : true is successful to write sensor serial number 150 | */ 151 | bool Cypress::writeSerialNumber(std::string number) 152 | { 153 | uint8_t data[32] = { "\0" }; 154 | uint16_t address = ADDRESS_SERIAL_NUMBER; 155 | *(uint16_t*)data = address; 156 | for (int i = 0; i < number.size(); i++) 157 | data[i+2] = number.at(i); 158 | return usbControl(0x21, 0x1, 0x500, 0x300, data, number.size()+2); 159 | } 160 | 161 | /* 162 | * @function: getSerialNumber 163 | * @brief : get the serial number of sensor 164 | * @input : 165 | * @output : 166 | * @return : sensor serial number 167 | */ 168 | std::string Cypress::getSerialNumber() 169 | { 170 | char data_read[32] = { "\0" }; 171 | uint16_t address = ADDRESS_SERIAL_NUMBER; 172 | *(uint16_t*)data_read = address; 173 | if (usbControl(0x21, 0x01, 0x600, 0x300, (uint8_t*)data_read, 32)) 174 | { 175 | if (usbControl(0xA1, 0x81, 0x600, 0x300, (uint8_t*)data_read, 32)) 176 | { 177 | return(std::string(&data_read[2])); 178 | } 179 | } 180 | return std::string(""); 181 | } 182 | 183 | /* 184 | * @function: getFirmwareVersion 185 | * @brief : get firmware version 186 | * @input : 187 | * @output : 188 | * @return : firmware version 189 | */ 190 | std::string Cypress::getFirmwareVersion() 191 | { 192 | char data_read[5] = { "\0" }; 193 | uint16_t address = ADDRESS_FIRMWARE_VERSION; 194 | *(uint16_t*)data_read = address; 195 | if (usbControl(0x21, 0x01, 0x600, 0x300, (uint8_t*)data_read, 4)) 196 | { 197 | if (usbControl(0xA1, 0x81, 0x600, 0x300, (uint8_t*)data_read, 4)) 198 | { 199 | std::string str; 200 | std::stringstream ss; 201 | ss << static_cast(data_read[3]); 202 | ss << "."; 203 | ss << static_cast(data_read[2]); 204 | str = std::string(ss.str()); 205 | 206 | return str; 207 | } 208 | } 209 | return std::string(""); 210 | } 211 | 212 | /* 213 | * @function: getFirmwareDate 214 | * @brief : get firmware date 215 | * @input : 216 | * @output : 217 | * @return : firmware date 218 | */ 219 | std::string Cypress::getFirmwareDate() 220 | { 221 | char dataRead[32] = { "\0" }; 222 | uint16_t address = ADDRESS_FIRMWARE_DATE; 223 | *(uint16_t*)dataRead = address; 224 | if (usbControl(0x21, 0x01, 0x600, 0x300, (uint8_t*)dataRead, 32)) 225 | { 226 | if (usbControl(0xA1, 0x81, 0x600, 0x300, (uint8_t*)dataRead, 32)) 227 | { 228 | return(std::string(&dataRead[2])); 229 | } 230 | } 231 | return std::string(""); 232 | } 233 | --------------------------------------------------------------------------------