diff --git a/device/camera_reader/CMakeLists.txt b/device/camera_reader/CMakeLists.txt deleted file mode 100644 index 042a958..0000000 --- a/device/camera_reader/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -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( REQUIRED) - -# Create executable -add_executable(camera_reader src/camera_reader.cpp src/main.cpp) - -target_include_directories(camera_reader - PUBLIC - $ - $ - ${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() diff --git a/device/camera_reader/camera_reader/__init__.py b/device/camera_reader/camera_reader/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/device/camera_reader/camera_reader/camera.py b/device/camera_reader/camera_reader/camera.py new file mode 100644 index 0000000..66d1397 --- /dev/null +++ b/device/camera_reader/camera_reader/camera.py @@ -0,0 +1,35 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +import cv2 +from cv_bridge import CvBridge + + +class Camera(Node): + def __init__(self): + super().__init__('camera') + self.publisher_=self.create_publisher(Image, 'camera_image', 10) + self.timer=self.create_timer(0.1, self.timer_callback) + self.cap=cv2.VideoCapture(0) + self.bridge=CvBridge() + if not self.cap.isOpened(): + self.get_logger().error('Failed to open camera') + rclpy.shutdown() + + def timer_callback(self): + ret,frame=self.cap.read() + if ret: + msg=self.bridge.cv2_to_imgmsg(frame, "bgr8") + self.publisher_.publish(msg) + else: + self.get_logger().error('Failed to capture image') + +def main(args=None): + rclpy.init(args=args) + camera=Camera() + rclpy.spin(camera) + camera.destroy_node() + rclpy.shutdown() + +if __name__=="__main__": + main() diff --git a/device/camera_reader/include/camera_reader/camera_reader.hpp b/device/camera_reader/include/camera_reader/camera_reader.hpp deleted file mode 100644 index f206ae6..0000000 --- a/device/camera_reader/include/camera_reader/camera_reader.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#include -#include -#include -#include -#include - -class CameraReader : public rclcpp::Node { -public: - CameraReader(); - -private: - void timer_callback(); - - image_transport::Publisher publisher_; - rclcpp::TimerBase::SharedPtr timer_; - cv::VideoCapture cap_; -}; diff --git a/device/camera_reader/package.xml b/device/camera_reader/package.xml index 4a50296..c0c2180 100644 --- a/device/camera_reader/package.xml +++ b/device/camera_reader/package.xml @@ -3,21 +3,15 @@ camera_reader 0.0.0 - ROS 2 package for receiving images - hiro + Camera + hiro MIT License - ament_cmake - rclcpp - sensor_msgs - cv_bridge - image_transport - rclcpp + rclpy sensor_msgs cv_bridge - image_transport - ament_cmake + ament_python diff --git a/device/camera_reader/resource/camera_reader b/device/camera_reader/resource/camera_reader new file mode 100644 index 0000000..e69de29 diff --git a/device/camera_reader/setup.cfg b/device/camera_reader/setup.cfg new file mode 100644 index 0000000..231851f --- /dev/null +++ b/device/camera_reader/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/camera_reader +[install] +install_scripts=$base/lib/camera_reader diff --git a/device/camera_reader/setup.py b/device/camera_reader/setup.py new file mode 100644 index 0000000..c7dc50c --- /dev/null +++ b/device/camera_reader/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'camera_reader' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages',['resource/' + package_name]), + ('share/' + package_name,['package.xml']), + (os.path.join('share','package_name','launch'),glob(os.path.join('launch','*'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='hiro', + maintainer_email='yoshikawa.h.ah@m.titech.ac.jp', + description='Camera for ROS2', + license='MIT license', + tests_require=[], + entry_points={ + 'console_scripts': [ + 'camera=camera_reader.camera:main', + ], + }, +) \ No newline at end of file diff --git a/device/camera_reader/src/camera_reader.cpp b/device/camera_reader/src/camera_reader.cpp deleted file mode 100644 index deb6345..0000000 --- a/device/camera_reader/src/camera_reader.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#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); - } -} diff --git a/device/camera_reader/src/main.cpp b/device/camera_reader/src/main.cpp deleted file mode 100644 index 3077468..0000000 --- a/device/camera_reader/src/main.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "camera_reader/camera_reader.hpp" -#include - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -}