Merge pull request #13 from rogy-AquaLab/camera_reader

device/camera_reader/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.8)
+project(camera_reader)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(OpenCV REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+# Create executable
+add_executable(camera_reader src/camera_reader.cpp src/main.cpp)
+
+target_include_directories(camera_reader
+ PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>
+ ${OpenCV_INCLUDE_DIRS}
+)
+
+# Link dependencies
+ament_target_dependencies(camera_reader rclcpp sensor_msgs cv_bridge image_transport)
+
+target_link_libraries(camera_reader ${OpenCV_LIBRARIES})
+
+# Install targets
+install(TARGETS
+ camera_reader
+ DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch
+ DESTINATION share/${PROJECT_NAME})
+
+ament_package()

device/camera_reader/README.md

@@ -0,0 +1,13 @@
+# camera_reader
+
+USBカメラの映像を取得
+
+## Nodes
+
+- `Camera`: カメラ映像を取得
+
+## Executables
+
+- `camera`: `Camera`Nodeをspin
+
+## Launches

device/camera_reader/include/camera_reader/camera_reader.hpp

@@ -0,0 +1,17 @@
+#include <cv_bridge/cv_bridge.hpp>
+#include <image_transport/image_transport.hpp>
+#include <opencv2/opencv.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+class CameraReader : public rclcpp::Node {
+public:
+ CameraReader();
+
+private:
+ void timer_callback();
+
+ image_transport::Publisher publisher_;
+ rclcpp::TimerBase::SharedPtr timer_;
+ cv::VideoCapture cap_;
+};

device/camera_reader/launch/camera_reader_launch.py

@@ -0,0 +1,12 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package="camera_reader", 
+ executable="camera_reader", 
+ namespace="device",
+ remappings=[("/device/camera/image", "/packet/camera/image")])
+ ])

device/camera_reader/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>camera_reader</name>
+ <version>0.0.0</version>
+ <description>ROS 2 package for receiving images</description>
+ <maintainer email="[email protected]">hiro</maintainer>
+ <license>MIT License</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <build_depend>rclcpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>cv_bridge</build_depend>
+ <build_depend>image_transport</build_depend>
+ <exec_depend>rclcpp</exec_depend>
+ <exec_depend>sensor_msgs</exec_depend>
+ <exec_depend>cv_bridge</exec_depend>
+ <exec_depend>image_transport</exec_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>

device/camera_reader/src/camera_reader.cpp

@@ -0,0 +1,29 @@
+#include "camera_reader/camera_reader.hpp"
+
+CameraReader::CameraReader() : Node("camera") {
+ publisher_ = image_transport::create_publisher(this, "camera/image");
+
+ // Open the default camera
+ cap_.open(0); // 0 is typically the default camera index
+
+ if (!cap_.isOpened()) {
+ RCLCPP_ERROR(this->get_logger(), "Failed to open camera");
+ return;
+ }
+
+ timer_ = this->create_wall_timer(
+ std::chrono::milliseconds(33), std::bind(&CameraReader::timer_callback, this)
+ );
+}
+
+void CameraReader::timer_callback() {
+ cv::Mat frame;
+ if (!cap_.read(frame)) {
+ return;
+ }
+ if (!frame.empty()) {
+ const auto msg
+ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg();
+ publisher_.publish(msg);
+ }
+}

device/camera_reader/src/main.cpp

@@ -0,0 +1,10 @@
+#include "camera_reader/camera_reader.hpp"
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared<CameraReader>();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}