├── .dockerignore ├── .github └── workflows │ └── build_test.yml ├── .gitignore ├── CMakeLists.txt ├── Dockerfile ├── README.md ├── include └── opencv_cam │ ├── camera_context.hpp │ ├── opencv_cam_node.hpp │ └── subscriber_node.hpp ├── launch └── composition_launch.py ├── package.xml ├── src ├── ipc_test_main.cpp ├── opencv_cam_main.cpp ├── opencv_cam_node.cpp └── subscriber_node.cpp └── workspace.repos /.dockerignore: -------------------------------------------------------------------------------- 1 | .idea 2 | cmake-build-* 3 | .cmake-build-* 4 | -------------------------------------------------------------------------------- /.github/workflows/build_test.yml: -------------------------------------------------------------------------------- 1 | name: ROS2 CI 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - 'master' 7 | push: 8 | branches: 9 | - 'master' 10 | 11 | jobs: 12 | test_environment: 13 | runs-on: [ubuntu-latest] 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | ros_distribution: 18 | - foxy 19 | - galactic 20 | - humble 21 | - iron 22 | include: 23 | - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-foxy-ros-base-latest 24 | ros_distribution: foxy 25 | - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-ros-base-latest 26 | ros_distribution: galactic 27 | - ros_distribution: humble 28 | docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest 29 | - ros_distribution: iron 30 | docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest 31 | container: 32 | image: ${{ matrix.docker_image }} 33 | steps: 34 | - name: setup directories 35 | run: mkdir -p ros_ws/src 36 | - name: checkout 37 | uses: actions/checkout@v2 38 | with: 39 | path: ros_ws/src 40 | - name: build and test 41 | uses: ros-tooling/action-ros-ci@master 42 | with: 43 | package-name: opencv_cam 44 | target-ros2-distro: ${{ matrix.ros_distribution }} 45 | vcs-repo-file-url: "https://raw.githubusercontent.com/${{github.repository}}/${{github.sha}}/workspace.repos" -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | cmake-build-* 3 | .cmake-build-* 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(opencv_cam) 3 | 4 | # Default to C++14 5 | if (NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif () 8 | 9 | if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif () 12 | 13 | # Try for OpenCV 4.X, but settle for whatever is installed 14 | find_package(OpenCV 4 QUIET) 15 | if (NOT OpenCV_FOUND) 16 | find_package(OpenCV REQUIRED) 17 | endif () 18 | message(STATUS "Found OpenCV version ${OpenCV_VERSION}") 19 | 20 | find_package(ament_cmake REQUIRED) 21 | find_package(camera_calibration_parsers REQUIRED) 22 | find_package(class_loader REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | find_package(rclcpp_components REQUIRED) 25 | find_package(ros2_shared REQUIRED) 26 | find_package(sensor_msgs REQUIRED) 27 | 28 | # Package includes not needed for CMake >= 2.8.11 29 | include_directories( 30 | include 31 | ${ros2_shared_INCLUDE_DIRS} 32 | ) 33 | 34 | # Create ament index resource which references the libraries in the binary dir 35 | set(node_plugins "") 36 | 37 | #============= 38 | # OpenCV camera node 39 | #============= 40 | 41 | add_library( 42 | opencv_cam_node SHARED 43 | src/opencv_cam_node.cpp 44 | ) 45 | target_compile_definitions( 46 | opencv_cam_node 47 | PRIVATE "COMPOSITION_BUILDING_DLL" 48 | ) 49 | ament_target_dependencies( 50 | opencv_cam_node 51 | camera_calibration_parsers 52 | class_loader 53 | OpenCV 54 | rclcpp 55 | rclcpp_components 56 | sensor_msgs 57 | ros2_shared 58 | ) 59 | rclcpp_components_register_nodes(opencv_cam_node "opencv_cam::OpencvCamNode") 60 | set(node_plugins "${node_plugins}opencv_cam::OpencvCamNode;$\n") 61 | 62 | #============= 63 | # Test subscriber node 64 | #============= 65 | 66 | add_library( 67 | subscriber_node SHARED 68 | src/subscriber_node.cpp 69 | ) 70 | target_compile_definitions( 71 | subscriber_node 72 | PRIVATE "COMPOSITION_BUILDING_DLL" 73 | ) 74 | ament_target_dependencies( 75 | subscriber_node 76 | class_loader 77 | rclcpp 78 | rclcpp_components 79 | sensor_msgs 80 | ) 81 | rclcpp_components_register_nodes(subscriber_node "opencv_cam::ImageSubscriberNode") 82 | set(node_plugins "${node_plugins}opencv_cam::ImageSubscriberNode;$\n") 83 | 84 | #============= 85 | # OpenCV camera main, IPC=true 86 | #============= 87 | 88 | add_executable( 89 | opencv_cam_main 90 | src/opencv_cam_main.cpp 91 | ) 92 | target_link_libraries( 93 | opencv_cam_main 94 | opencv_cam_node 95 | ) 96 | ament_target_dependencies( 97 | opencv_cam_main 98 | rclcpp 99 | ) 100 | 101 | #============= 102 | # Manual composition of camera and subscriber nodes, IPC=true 103 | #============= 104 | 105 | add_executable( 106 | ipc_test_main 107 | src/ipc_test_main.cpp 108 | ) 109 | target_link_libraries( 110 | ipc_test_main 111 | opencv_cam_node 112 | subscriber_node 113 | ) 114 | ament_target_dependencies( 115 | ipc_test_main 116 | rclcpp 117 | ) 118 | 119 | #============= 120 | # Export 121 | # Best practice, see https://discourse.ros.org/t/ament-best-practice-for-sharing-libraries/3602 122 | #============= 123 | 124 | ament_export_dependencies(class_loader) 125 | 126 | ament_export_include_directories(include) 127 | 128 | ament_export_targets(export_opencv_cam_node export_subscriber_node) 129 | 130 | ament_export_libraries(gscam_node subscriber_node) 131 | 132 | ament_package() 133 | 134 | #============= 135 | # Install 136 | #============= 137 | 138 | # Install nodes 139 | install( 140 | TARGETS opencv_cam_node 141 | EXPORT export_opencv_cam_node 142 | ARCHIVE DESTINATION lib 143 | LIBRARY DESTINATION lib 144 | RUNTIME DESTINATION bin 145 | ) 146 | 147 | install( 148 | TARGETS subscriber_node 149 | EXPORT export_subscriber_node 150 | ARCHIVE DESTINATION lib 151 | LIBRARY DESTINATION lib 152 | RUNTIME DESTINATION bin 153 | ) 154 | 155 | # Install executables 156 | install( 157 | TARGETS opencv_cam_main ipc_test_main 158 | DESTINATION lib/${PROJECT_NAME} 159 | ) 160 | 161 | # Install various directories 162 | install( 163 | DIRECTORY launch 164 | DESTINATION share/${PROJECT_NAME} 165 | ) 166 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # Build the image: 2 | # docker build --build-arg ROS_DISTRO=$ROS_DISTRO --tag opencv_cam:$ROS_DISTRO . 3 | 4 | # Run a test using /dev/video0: 5 | # docker run -it --device=/dev/video0:/dev/video0 opencv_cam:$ROS_DISTRO 6 | 7 | # Interactive session with Rocker (https://github.com/osrf/rocker): 8 | # rocker --x11 --nvidia --devices /dev/video0 -- opencv_cam:$ROS_DISTRO bash 9 | 10 | ARG ROS_DISTRO=foxy 11 | 12 | FROM osrf/ros:${ROS_DISTRO}-desktop 13 | 14 | ARG ROS_DISTRO 15 | 16 | RUN apt-get update && apt-get upgrade -y 17 | 18 | RUN apt-get install wget 19 | 20 | WORKDIR /work/opencv_cam_ws 21 | 22 | ARG ROS2_SHARED_BRANCH=master 23 | RUN git clone https://github.com/ptrmu/ros2_shared.git -b $ROS2_SHARED_BRANCH 24 | 25 | # Changes to opencv_cam will cause a re-build 26 | COPY . src/opencv_cam/ 27 | 28 | RUN rosdep install -y --from-paths . --ignore-src 29 | 30 | RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build" 31 | 32 | # Requires a webcam on /dev/video0: 33 | CMD ["/bin/bash", "-c", "source install/setup.bash && ros2 run opencv_cam opencv_cam_main"] 34 | 35 | # View images: 36 | # ros2 run image_tools showimage --ros-args -p reliability:=reliable -r image:=/image_raw -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # opencv_cam 2 | 3 | A simple [ROS2](https://index.ros.org/doc/ros2/) camera driver based on [OpenCV](https://opencv.org/). 4 | 5 | Supports [ROS2 intra-process comms](https://index.ros.org//doc/ros2/Tutorials/Intra-Process-Communication/). 6 | 7 | Requires [ROS2](https://index.ros.org/doc/ros2/Installation/) and 8 | `ros-$ROS_DISTRO-camera-calibration-parsers`. 9 | Builds for ROS2 Foxy, Galactic, Humble and Iron. 10 | 11 | ## Install and build 12 | 13 | ~~~ 14 | mkdir ~/ros2/opencv_cam_ws/src 15 | cd ~/ros2/opencv_cam_ws/src 16 | git clone https://github.com/clydemcqueen/opencv_cam.git 17 | git clone https://github.com/ptrmu/ros2_shared.git 18 | cd ~/ros2/opencv_cam_ws/ 19 | source /opt/ros/$ROS_DISTRO/setup.bash 20 | colcon build 21 | ~~~ 22 | 23 | ## Usage 24 | 25 | Default is to publish images from `/dev/video0`: 26 | ~~~ 27 | ros2 run opencv_cam opencv_cam_main 28 | ~~~ 29 | 30 | A more complex example: 31 | ~~~ 32 | ros2 run opencv_cam opencv_cam_main --ros-args --remap /image_raw:=/my_camera/image_raw --params-file opencv_cam_params.yaml 33 | ~~~ 34 | ... where opencv_cam_params.yaml is: 35 | ~~~ 36 | /opencv_cam: 37 | ros__parameters: 38 | file: True 39 | filename: 'my_camera.MOV' 40 | camera_info_path: 'my_camera_info.ini' 41 | camera_frame_id: 'my_camera' 42 | ~~~ 43 | 44 | ### Intra-process comms 45 | 46 | IPC test -- CLI composition: 47 | ~~~ 48 | # First shell 49 | ros2 run rclcpp_components component_container 50 | 51 | # Second shell (ignore the deprecation warning, see https://github.com/ros2/ros2cli/issues/336) 52 | ros2 component load /ComponentManager opencv_cam opencv_cam::ImageSubscriberNode -e use_intra_process_comms:=true 53 | ros2 component load /ComponentManager opencv_cam opencv_cam::OpencvCamNode -e use_intra_process_comms:=true 54 | ~~~ 55 | 56 | Launch file composition: 57 | ~~~ 58 | ros2 launch opencv_cam composition_launch.py 59 | ~~~ 60 | 61 | Manual composition -- handy for debugging: 62 | ~~~ 63 | ros2 run opencv_cam ipc_test_main 64 | ~~~ 65 | 66 | ## Parameters 67 | 68 | | Parameter | Type | Default | Notes | 69 | |---|---|---|---| 70 | | file | bool | False | Read from file vs. read from device | 71 | | fps | int | 0 | Framerate. Specify 0 to publish at the recorded (file) or default (device) framerate | 72 | | filename | string | "" | Filename, ignored if file is False | 73 | | index | int | 0 | Device index, 0 for /dev/video0. Ignored if file is True | 74 | | width | int | 0 | Device width in pixels. Specify 0 for default. Ignored if file is True | 75 | | height | int | 0 | Device width in pixels. Specify 0 for default. Ignored if file is True | 76 | | camera_info_path | string | "info.ini" | Camera info path | 77 | | camera_frame | string | "camera_frame" | Camera frame id | 78 | 79 | ## Camera info file formats 80 | 81 | Uses the [ROS standard camera calibration formats](http://wiki.ros.org/camera_calibration_parsers). 82 | Files must end in `.ini` or `.yaml`. -------------------------------------------------------------------------------- /include/opencv_cam/camera_context.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OPENCV_CAM_CONTEXT_HPP 2 | #define OPENCV_CAM_CONTEXT_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "ros2_shared/context_macros.hpp" 8 | 9 | namespace opencv_cam 10 | { 11 | 12 | #define OPENCV_CAM_ALL_PARAMS \ 13 | CXT_MACRO_MEMBER(file, bool, false) /* Read from file vs. read from device */ \ 14 | CXT_MACRO_MEMBER(fps, int, 0) /* Desired frames per second */ \ 15 | \ 16 | CXT_MACRO_MEMBER(filename, std::string, "") /* Filename */ \ 17 | \ 18 | CXT_MACRO_MEMBER(index, int, 0) /* Device index, see cv::VideoCaptureAPIs */ \ 19 | CXT_MACRO_MEMBER(width, int, 0) /* Device width */ \ 20 | CXT_MACRO_MEMBER(height, int, 0) /* Device height */ \ 21 | \ 22 | CXT_MACRO_MEMBER(camera_info_path, std::string, "info.ini") /* Camera info path */ \ 23 | CXT_MACRO_MEMBER(camera_frame_id, std::string, "camera_frame") /* Camera frame id */ \ 24 | /* End of list */ 25 | 26 | 27 | struct CameraContext 28 | { 29 | #undef CXT_MACRO_MEMBER 30 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 31 | CXT_MACRO_DEFINE_MEMBERS(OPENCV_CAM_ALL_PARAMS) 32 | }; 33 | 34 | } // namespace opencv_cam 35 | 36 | #endif // OPENCV_CAM_CONTEXT_HPP 37 | -------------------------------------------------------------------------------- /include/opencv_cam/opencv_cam_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OPENCV_CAM_HPP 2 | #define OPENCV_CAM_HPP 3 | 4 | 5 | #include "opencv2/highgui/highgui.hpp" 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "sensor_msgs/msg/camera_info.hpp" 8 | #include "sensor_msgs/msg/image.hpp" 9 | 10 | #include "opencv_cam/camera_context.hpp" 11 | 12 | namespace opencv_cam 13 | { 14 | 15 | class OpencvCamNode : public rclcpp::Node 16 | { 17 | CameraContext cxt_; 18 | 19 | std::thread thread_; 20 | std::atomic canceled_; 21 | 22 | std::shared_ptr capture_; 23 | sensor_msgs::msg::CameraInfo camera_info_msg_; 24 | 25 | int publish_fps_; 26 | rclcpp::Time next_stamp_; 27 | 28 | rclcpp::Publisher::SharedPtr image_pub_; 29 | rclcpp::Publisher::SharedPtr camera_info_pub_; 30 | 31 | public: 32 | 33 | explicit OpencvCamNode(const rclcpp::NodeOptions &options); 34 | 35 | ~OpencvCamNode() override; 36 | 37 | private: 38 | 39 | void validate_parameters(); 40 | 41 | void loop(); 42 | }; 43 | 44 | } // namespace opencv_cam 45 | 46 | #endif //OPENCV_CAM_HPP 47 | -------------------------------------------------------------------------------- /include/opencv_cam/subscriber_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_SUBSCRIBER_HPP 2 | #define SIMPLE_SUBSCRIBER_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "sensor_msgs/msg/image.hpp" 6 | 7 | namespace opencv_cam 8 | { 9 | 10 | // Node that subscribes to a topic, used for testing composition and IPC 11 | class ImageSubscriberNode : public rclcpp::Node 12 | { 13 | rclcpp::Subscription::SharedPtr sub_; 14 | 15 | public: 16 | 17 | explicit ImageSubscriberNode(const rclcpp::NodeOptions &options); 18 | }; 19 | 20 | } // namespace opencv_cam 21 | 22 | #endif //SIMPLE_SUBSCRIBER_HPP -------------------------------------------------------------------------------- /launch/composition_launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Dynamically compose OpencvCamNode and ImageSubscriberNode in a component_container. 3 | 4 | Limitations of this container: 5 | -- stdout is not set to flush after each line, so the most recent log messages may be delayed 6 | """ 7 | 8 | import launch 9 | from launch_ros.actions import ComposableNodeContainer 10 | from launch_ros.descriptions import ComposableNode 11 | 12 | 13 | def generate_launch_description(): 14 | movie = 'test.mov' 15 | camera_info_path = 'info.ini' 16 | 17 | container = ComposableNodeContainer( 18 | name='my_container', 19 | namespace='', 20 | package='rclcpp_components', 21 | executable='component_container', 22 | composable_node_descriptions=[ 23 | ComposableNode( 24 | package='opencv_cam', 25 | plugin='opencv_cam::OpencvCamNode', 26 | name='image_publisher', 27 | parameters=[{ 28 | 'file': True, 29 | 'filename': movie, 30 | 'camera_info_path': camera_info_path, 31 | }], 32 | extra_arguments=[{'use_intra_process_comms': True}], 33 | ), 34 | ComposableNode( 35 | package='opencv_cam', 36 | plugin='opencv_cam::ImageSubscriberNode', 37 | name='image_subscriber', 38 | extra_arguments=[{'use_intra_process_comms': True}], 39 | ), 40 | ], 41 | output='screen', 42 | ) 43 | 44 | return launch.LaunchDescription([container]) 45 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | opencv_cam 6 | 0.2.0 7 | OpenCV camera driver 8 | 9 | Clyde McQueen 10 | BSD 11 | 12 | https://github.com/clydemcqueen/opencv_cam.git 13 | https://github.com/clydemcqueen/opencv_cam/issues 14 | 15 | Clyde McQueen 16 | Peter Mullen 17 | 18 | ament_cmake 19 | 20 | camera_calibration_parsers 21 | class_loader 22 | libopencv-dev 23 | rclcpp 24 | ros2_shared 25 | sensor_msgs 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/ipc_test_main.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv_cam/opencv_cam_node.hpp" 2 | #include "opencv_cam/subscriber_node.hpp" 3 | 4 | // Manually compose OpencvCamNode and ImageSubscriberNode with use_intra_process_comms=true 5 | 6 | int main(int argc, char **argv) 7 | { 8 | // Force flush of the stdout buffer 9 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 10 | 11 | // Init ROS 12 | rclcpp::init(argc, argv); 13 | 14 | // Create single-threaded executor 15 | rclcpp::executors::SingleThreadedExecutor executor; 16 | 17 | // Create and add camera node 18 | rclcpp::NodeOptions options{}; 19 | options.use_intra_process_comms(true); 20 | auto camera_node = std::make_shared(options); 21 | executor.add_node(camera_node); 22 | 23 | // Create and add subscriber node 24 | auto subscriber_node = std::make_shared(options); 25 | executor.add_node(subscriber_node); 26 | 27 | // Spin until rclcpp::ok() returns false 28 | executor.spin(); 29 | 30 | // Shut down ROS 31 | rclcpp::shutdown(); 32 | 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /src/opencv_cam_main.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv_cam/opencv_cam_node.hpp" 2 | 3 | // Launch OpencvCamNode with use_intra_process_comms=true 4 | 5 | int main(int argc, char **argv) 6 | { 7 | // Force flush of the stdout buffer 8 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 9 | 10 | // Init ROS 11 | rclcpp::init(argc, argv); 12 | 13 | // Create single-threaded executor 14 | rclcpp::executors::SingleThreadedExecutor executor; 15 | 16 | // Create and add camera node 17 | rclcpp::NodeOptions options{}; 18 | options.use_intra_process_comms(true); 19 | auto node = std::make_shared(options); 20 | executor.add_node(node); 21 | 22 | // Spin until rclcpp::ok() returns false 23 | executor.spin(); 24 | 25 | // Shut down ROS 26 | rclcpp::shutdown(); 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /src/opencv_cam_node.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv_cam/opencv_cam_node.hpp" 2 | 3 | #include 4 | 5 | #include "camera_calibration_parsers/parse.hpp" 6 | 7 | namespace opencv_cam 8 | { 9 | 10 | std::string mat_type2encoding(int mat_type) 11 | { 12 | switch (mat_type) { 13 | case CV_8UC1: 14 | return "mono8"; 15 | case CV_8UC3: 16 | return "bgr8"; 17 | case CV_16SC1: 18 | return "mono16"; 19 | case CV_8UC4: 20 | return "rgba8"; 21 | default: 22 | throw std::runtime_error("unsupported encoding type"); 23 | } 24 | } 25 | 26 | OpencvCamNode::OpencvCamNode(const rclcpp::NodeOptions &options) : 27 | Node("opencv_cam", options), 28 | canceled_(false) 29 | { 30 | RCLCPP_INFO(get_logger(), "use_intra_process_comms=%d", options.use_intra_process_comms()); 31 | 32 | // Initialize parameters 33 | #undef CXT_MACRO_MEMBER 34 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER((*this), cxt_, n, t, d) 35 | CXT_MACRO_INIT_PARAMETERS(OPENCV_CAM_ALL_PARAMS, validate_parameters) 36 | 37 | // Register for parameter changed. NOTE at this point nothing is done when parameters change. 38 | #undef CXT_MACRO_MEMBER 39 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 40 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED((*this), cxt_, OPENCV_CAM_ALL_PARAMS, validate_parameters) 41 | 42 | // Log the current parameters 43 | #undef CXT_MACRO_MEMBER 44 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_SORTED_PARAMETER(cxt_, n, t, d) 45 | CXT_MACRO_LOG_SORTED_PARAMETERS(RCLCPP_INFO, get_logger(), "opencv_cam Parameters", OPENCV_CAM_ALL_PARAMS) 46 | 47 | // Check that all command line parameters are registered 48 | #undef CXT_MACRO_MEMBER 49 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_CHECK_CMDLINE_PARAMETER(n, t, d) 50 | CXT_MACRO_CHECK_CMDLINE_PARAMETERS((*this), OPENCV_CAM_ALL_PARAMS) 51 | 52 | RCLCPP_INFO(get_logger(), "OpenCV version %d", CV_VERSION_MAJOR); 53 | 54 | // Open file or device 55 | if (cxt_.file_) { 56 | capture_ = std::make_shared(cxt_.filename_); 57 | 58 | if (!capture_->isOpened()) { 59 | RCLCPP_ERROR(get_logger(), "cannot open file %s", cxt_.filename_.c_str()); 60 | return; 61 | } 62 | 63 | if (cxt_.fps_ > 0) { 64 | // Publish at the specified rate 65 | publish_fps_ = cxt_.fps_; 66 | } else { 67 | // Publish at the recorded rate 68 | publish_fps_ = static_cast(capture_->get(cv::CAP_PROP_FPS)); 69 | } 70 | 71 | double width = capture_->get(cv::CAP_PROP_FRAME_WIDTH); 72 | double height = capture_->get(cv::CAP_PROP_FRAME_HEIGHT); 73 | RCLCPP_INFO(get_logger(), "file %s open, width %g, height %g, publish fps %d", 74 | cxt_.filename_.c_str(), width, height, publish_fps_); 75 | 76 | next_stamp_ = now(); 77 | 78 | } else { 79 | capture_ = std::make_shared(cxt_.index_); 80 | 81 | if (!capture_->isOpened()) { 82 | RCLCPP_ERROR(get_logger(), "cannot open device %d", cxt_.index_); 83 | return; 84 | } 85 | 86 | if (cxt_.height_ > 0) { 87 | capture_->set(cv::CAP_PROP_FRAME_HEIGHT, cxt_.height_); 88 | } 89 | 90 | if (cxt_.width_ > 0) { 91 | capture_->set(cv::CAP_PROP_FRAME_WIDTH, cxt_.width_); 92 | } 93 | 94 | if (cxt_.fps_ > 0) { 95 | capture_->set(cv::CAP_PROP_FPS, cxt_.fps_); 96 | } 97 | 98 | double width = capture_->get(cv::CAP_PROP_FRAME_WIDTH); 99 | double height = capture_->get(cv::CAP_PROP_FRAME_HEIGHT); 100 | double fps = capture_->get(cv::CAP_PROP_FPS); 101 | RCLCPP_INFO(get_logger(), "device %d open, width %g, height %g, device fps %g", 102 | cxt_.index_, width, height, fps); 103 | } 104 | 105 | assert(!cxt_.camera_info_path_.empty()); // readCalibration will crash if file_name is "" 106 | std::string camera_name; 107 | if (camera_calibration_parsers::readCalibration(cxt_.camera_info_path_, camera_name, camera_info_msg_)) { 108 | RCLCPP_INFO(get_logger(), "got camera info for '%s'", camera_name.c_str()); 109 | camera_info_msg_.header.frame_id = cxt_.camera_frame_id_; 110 | camera_info_pub_ = create_publisher("camera_info", 10); 111 | } else { 112 | RCLCPP_ERROR(get_logger(), "cannot get camera info, will not publish"); 113 | camera_info_pub_ = nullptr; 114 | } 115 | 116 | image_pub_ = create_publisher("image_raw", 10); 117 | 118 | // Run loop on it's own thread 119 | thread_ = std::thread(std::bind(&OpencvCamNode::loop, this)); 120 | 121 | RCLCPP_INFO(get_logger(), "start publishing"); 122 | } 123 | 124 | OpencvCamNode::~OpencvCamNode() 125 | { 126 | // Stop loop 127 | canceled_.store(true); 128 | if (thread_.joinable()) { 129 | thread_.join(); 130 | } 131 | } 132 | 133 | void OpencvCamNode::validate_parameters() 134 | {} 135 | 136 | void OpencvCamNode::loop() 137 | { 138 | cv::Mat frame; 139 | 140 | while (rclcpp::ok() && !canceled_.load()) { 141 | // Read a frame, if this is a device block until a frame is available 142 | if (!capture_->read(frame)) { 143 | RCLCPP_INFO(get_logger(), "EOF, stop publishing"); 144 | break; 145 | } 146 | 147 | auto stamp = now(); 148 | 149 | // Avoid copying image message if possible 150 | sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image()); 151 | 152 | // Convert OpenCV Mat to ROS Image 153 | image_msg->header.stamp = stamp; 154 | image_msg->header.frame_id = cxt_.camera_frame_id_; 155 | image_msg->height = frame.rows; 156 | image_msg->width = frame.cols; 157 | image_msg->encoding = mat_type2encoding(frame.type()); 158 | image_msg->is_bigendian = false; 159 | image_msg->step = static_cast(frame.step); 160 | image_msg->data.assign(frame.datastart, frame.dataend); 161 | 162 | #undef SHOW_ADDRESS 163 | #ifdef SHOW_ADDRESS 164 | static int count = 0; 165 | RCLCPP_INFO(get_logger(), "%d, %p", count++, reinterpret_cast(image_msg.get())); 166 | #endif 167 | 168 | // Publish 169 | image_pub_->publish(std::move(image_msg)); 170 | if (camera_info_pub_) { 171 | camera_info_msg_.header.stamp = stamp; 172 | camera_info_pub_->publish(camera_info_msg_); 173 | } 174 | 175 | // Sleep if required 176 | if (cxt_.file_) { 177 | using namespace std::chrono_literals; 178 | next_stamp_ = next_stamp_ + rclcpp::Duration{1000000000ns / publish_fps_}; 179 | auto wait = next_stamp_ - stamp; 180 | if (wait.nanoseconds() > 0) { 181 | std::this_thread::sleep_for(static_cast(wait.nanoseconds())); 182 | } 183 | } 184 | } 185 | } 186 | 187 | } // namespace opencv_cam 188 | 189 | #include "rclcpp_components/register_node_macro.hpp" 190 | 191 | RCLCPP_COMPONENTS_REGISTER_NODE(opencv_cam::OpencvCamNode) -------------------------------------------------------------------------------- /src/subscriber_node.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv_cam/subscriber_node.hpp" 2 | 3 | namespace opencv_cam 4 | { 5 | 6 | ImageSubscriberNode::ImageSubscriberNode(const rclcpp::NodeOptions &options) : 7 | Node("image_subscriber", options) 8 | { 9 | RCLCPP_INFO(get_logger(), "use_intra_process_comms=%d", options.use_intra_process_comms()); 10 | 11 | sub_ = this->create_subscription( 12 | "image_raw", 1, 13 | [this](sensor_msgs::msg::Image::UniquePtr msg) 14 | { 15 | static bool receiving = false; 16 | 17 | if (!receiving) { 18 | receiving = true; 19 | RCLCPP_INFO(get_logger(), "receiving messages"); 20 | } 21 | 22 | #undef SLOW_DOWN 23 | #ifdef SLOW_DOWN 24 | // Overflow the pub and sub buffer 25 | std::this_thread::sleep_for (std::chrono::milliseconds(100)); 26 | #endif 27 | 28 | #undef SHOW_ADDRESS 29 | #ifdef SHOW_ADDRESS 30 | static int count = 0; 31 | RCLCPP_INFO(get_logger(), "%d, %p", count++, reinterpret_cast(msg.get())); 32 | #else 33 | (void) this; 34 | (void) msg; 35 | #endif 36 | }); 37 | 38 | RCLCPP_INFO(get_logger(), "ready"); 39 | } 40 | 41 | } // namespace opencv_cam 42 | 43 | #include "rclcpp_components/register_node_macro.hpp" 44 | 45 | RCLCPP_COMPONENTS_REGISTER_NODE(opencv_cam::ImageSubscriberNode) -------------------------------------------------------------------------------- /workspace.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | opencv_cam: 3 | type: git 4 | url: https://github.com/clydemcqueen/opencv_cam 5 | version: master 6 | ros2_shared: 7 | type: git 8 | url: https://github.com/ptrmu/ros2_shared 9 | version: master 10 | 11 | --------------------------------------------------------------------------------