From bec42db8442b2ab12052d25074c7df3699994b05 Mon Sep 17 00:00:00 2001 From: Lars Lorentz Ludvigsen Date: Sat, 17 Jul 2021 07:58:48 -0400 Subject: [PATCH 1/9] Adding timestamp to message --- camera_pkg/src/camera_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/camera_pkg/src/camera_node.cpp b/camera_pkg/src/camera_node.cpp index 5ba40b1..626f034 100644 --- a/camera_pkg/src/camera_node.cpp +++ b/camera_pkg/src/camera_node.cpp @@ -139,6 +139,9 @@ namespace MediaEng { void produceFrames() { while (produceFrames_) { deepracer_interfaces_pkg::msg::CameraMsg msg; + std_msgs::msg::Header header; + header.stamp = this->get_clock()->now(); + for (auto& cap : videoCaptureList_) { if (!cap.isOpened()) { continue; From 5f20d9020f47f2605f639e1206a375b572c31a15 Mon Sep 17 00:00:00 2001 From: Lars Lorentz Ludvigsen Date: Wed, 29 Sep 2021 14:56:31 -0400 Subject: [PATCH 2/9] Changed parameters --- README.md | 6 +++-- camera_pkg/launch/camera_pkg_launch.py | 3 ++- camera_pkg/src/camera_node.cpp | 34 +++++++++++++++++--------- 3 files changed, 28 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index f68a5a5..a84b86a 100644 --- a/README.md +++ b/README.md @@ -121,7 +121,8 @@ The `camera_pkg_launch.py` included in this package provides an example demonstr executable='camera_node', name='camera_node' parameters=[ - {'resize_images': False} + {'resize_images': 4}, + {'display_topic_enable': True} ] ) ]) @@ -131,7 +132,8 @@ The `camera_pkg_launch.py` included in this package provides an example demonstr | Parameter name | Description | | ---------------- | ----------- | -| `resize_images` | Set to `True` or `False` depending on if you want to resize the images in camera_pkg | +| `resize_images` | Set to positive integer depending on if you want to resize the images in camera_pkg. Use 1 for no resizing | +| `display_topic_enable` | Set to true if you want to enable the `camera_pkg`/`display_mjpeg` topic | ## Node details diff --git a/camera_pkg/launch/camera_pkg_launch.py b/camera_pkg/launch/camera_pkg_launch.py index 90370a6..416cedd 100644 --- a/camera_pkg/launch/camera_pkg_launch.py +++ b/camera_pkg/launch/camera_pkg_launch.py @@ -26,7 +26,8 @@ def generate_launch_description(): executable='camera_node', name='camera_node', parameters=[ - {'resize_images': True} + {'resize_images': 4}, + {'display_topic_enable': True} ] ) ]) diff --git a/camera_pkg/src/camera_node.cpp b/camera_pkg/src/camera_node.cpp index 626f034..b6b8a8e 100644 --- a/camera_pkg/src/camera_node.cpp +++ b/camera_pkg/src/camera_node.cpp @@ -26,8 +26,8 @@ #include namespace MediaEng { - #define DEFAULT_IMAGE_WIDTH 160 - #define DEFAULT_IMAGE_HEIGHT 120 + #define DEFAULT_IMAGE_WIDTH 640 + #define DEFAULT_IMAGE_HEIGHT 480 class CameraNode : public rclcpp::Node { /// This class creates the camera_node responsible to read data from the cameras @@ -43,12 +43,18 @@ namespace MediaEng { CameraNode(const std::string & node_name, const std::vector cameraIdxList) : Node(node_name), produceFrames_(false), - resizeImages_(true) + downscaleImages_(4), + enableDisplayPub_(true) { RCLCPP_INFO(this->get_logger(), "%s started", node_name.c_str()); - this->declare_parameter("resize_images", resizeImages_); - // Update resizeImages boolean based on the parameter - resizeImages_ = this->get_parameter("resize_images").as_bool(); + this->declare_parameter("downscale_images", downscaleImages_); + // Update downscaleImages int based on the parameter + downscaleImages_ = this->get_parameter("downscale_images").as_int(); + + this->declare_parameter("display_topic_enable", enableDisplayPub_); + // Enable the Display Msg Topic + enableDisplayPub_ = this->get_parameter("display_topic_enable").as_bool(); + // Scan and load only valid streamers to Video Capture list. scanCameraIndex(cameraIdxList); @@ -60,7 +66,8 @@ namespace MediaEng { // This displays only the left/center camera images. Modify if the requirement is to publish images from both cameras. // The queue size for displayPub_ is set to 10 because web_video_server subscribes to the camera_node and the // image callback is blocked since it probably expects to send a frame which has been lost due to small publisher queue size of 1 earlier. - displayPub_ = this->create_publisher(DISPLAY_MSG_TOPIC, 10); + if (enableDisplayPub_) + displayPub_ = this->create_publisher(DISPLAY_MSG_TOPIC, 10); // Create a service to activate the publish of camera images. activateCameraService_ = this->create_service( @@ -153,8 +160,8 @@ namespace MediaEng { continue; } try { - if(resizeImages_) { - cv::resize(frame, frame, cv::Size(DEFAULT_IMAGE_WIDTH, DEFAULT_IMAGE_HEIGHT)); + if(downscaleImages_ > 1) { + cv::resize(frame, frame, cv::Size((int) DEFAULT_IMAGE_WIDTH / downscaleImages_, (int) DEFAULT_IMAGE_HEIGHT / downscaleImages_)); } msg.images.push_back(*(cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg().get())); } @@ -165,7 +172,8 @@ namespace MediaEng { } } try { - displayPub_->publish(msg.images.front()); + if (enableDisplayPub_) + displayPub_->publish(msg.images.front()); videoPub_->publish(msg); } catch (const std::exception &ex) { @@ -182,8 +190,10 @@ namespace MediaEng { rclcpp::Service::SharedPtr activateCameraService_; /// Boolean for starting and stopping the worker thread. std::atomic produceFrames_; - /// Boolean to resize images. - std::atomic resizeImages_; + /// Factor (int) to downscale images. + std::atomic downscaleImages_; + /// Boolean for enabling the display mpeg topic. + std::atomic enableDisplayPub_; /// List of OpenCV video capture object used to retrieve frames from the cameras. std::vector videoCaptureList_; /// List of valid camera indices identified after scanning. From cb5d5822bdb1736f7fa13ee02ae2ecbd8b623c74 Mon Sep 17 00:00:00 2001 From: Lars Lorentz Ludvigsen Date: Sat, 14 May 2022 07:36:04 -0400 Subject: [PATCH 3/9] Fixing backwards compatibility --- camera_pkg/launch/camera_pkg_launch.py | 4 ++- camera_pkg/src/camera_node.cpp | 40 ++++++++++++++++++++------ 2 files changed, 34 insertions(+), 10 deletions(-) diff --git a/camera_pkg/launch/camera_pkg_launch.py b/camera_pkg/launch/camera_pkg_launch.py index 416cedd..4ced03b 100644 --- a/camera_pkg/launch/camera_pkg_launch.py +++ b/camera_pkg/launch/camera_pkg_launch.py @@ -26,7 +26,9 @@ def generate_launch_description(): executable='camera_node', name='camera_node', parameters=[ - {'resize_images': 4}, + {'resize_images': True}, + {'resize_images_factor': 4}, + {'fps': 0}, {'display_topic_enable': True} ] ) diff --git a/camera_pkg/src/camera_node.cpp b/camera_pkg/src/camera_node.cpp index b6b8a8e..a3d2e6e 100644 --- a/camera_pkg/src/camera_node.cpp +++ b/camera_pkg/src/camera_node.cpp @@ -43,18 +43,29 @@ namespace MediaEng { CameraNode(const std::string & node_name, const std::vector cameraIdxList) : Node(node_name), produceFrames_(false), - downscaleImages_(4), - enableDisplayPub_(true) + resizeImages_(true), + resizeImagesFactor_(4), + enableDisplayPub_(true), + framesPerSecond_(0), + imageFrameId_(0) { RCLCPP_INFO(this->get_logger(), "%s started", node_name.c_str()); - this->declare_parameter("downscale_images", downscaleImages_); + + this->declare_parameter("resize_images", resizeImages_); + // Update resizeImages boolean based on the parameter + resizeImages_ = this->get_parameter("resize_images").as_bool(); + + this->declare_parameter("resize_images_factor", resizeImagesFactor_); // Update downscaleImages int based on the parameter - downscaleImages_ = this->get_parameter("downscale_images").as_int(); + resizeImagesFactor_ = this->get_parameter("resize_images_factor").as_int(); this->declare_parameter("display_topic_enable", enableDisplayPub_); // Enable the Display Msg Topic enableDisplayPub_ = this->get_parameter("display_topic_enable").as_bool(); + this->declare_parameter("fps", framesPerSecond_); + // Set the number of FPS you want - if 0 then leave to camera + framesPerSecond_ = this->get_parameter("fps").as_int(); // Scan and load only valid streamers to Video Capture list. scanCameraIndex(cameraIdxList); @@ -97,6 +108,11 @@ namespace MediaEng { videoCaptureList_.push_back(cap); videoCaptureList_.back().set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); + // Set the FPS + if (framesPerSecond_ > 0) { + RCLCPP_INFO(this->get_logger(), "[Camera Package] Setting %d fps on index %d.", framesPerSecond_.load(), idx); + videoCaptureList_.back().set(cv::CAP_PROP_FPS, framesPerSecond_.load()); + } // Add to valid video index list videoIndexList_.push_back(idx); } @@ -148,7 +164,7 @@ namespace MediaEng { deepracer_interfaces_pkg::msg::CameraMsg msg; std_msgs::msg::Header header; header.stamp = this->get_clock()->now(); - + header.frame_id = std::to_string(imageFrameId_++); for (auto& cap : videoCaptureList_) { if (!cap.isOpened()) { continue; @@ -160,8 +176,8 @@ namespace MediaEng { continue; } try { - if(downscaleImages_ > 1) { - cv::resize(frame, frame, cv::Size((int) DEFAULT_IMAGE_WIDTH / downscaleImages_, (int) DEFAULT_IMAGE_HEIGHT / downscaleImages_)); + if(resizeImages_) { + cv::resize(frame, frame, cv::Size((int) DEFAULT_IMAGE_WIDTH / resizeImagesFactor_, (int) DEFAULT_IMAGE_HEIGHT / resizeImagesFactor_)); } msg.images.push_back(*(cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg().get())); } @@ -190,10 +206,14 @@ namespace MediaEng { rclcpp::Service::SharedPtr activateCameraService_; /// Boolean for starting and stopping the worker thread. std::atomic produceFrames_; + /// Boolean to resize images. + std::atomic resizeImages_; /// Factor (int) to downscale images. - std::atomic downscaleImages_; + std::atomic resizeImagesFactor_; /// Boolean for enabling the display mpeg topic. - std::atomic enableDisplayPub_; + std::atomic enableDisplayPub_; + /// Int for defining the camera FPS. + std::atomic framesPerSecond_; /// List of OpenCV video capture object used to retrieve frames from the cameras. std::vector videoCaptureList_; /// List of valid camera indices identified after scanning. @@ -202,6 +222,8 @@ namespace MediaEng { std::thread videoWorker_; /// Camera index parameter to capture video frames from the specific camera. int cameraIndex_; + /// Frame ID + long imageFrameId_; }; } From 0267ba3b03cb90f3458959a54a768729e0bfc507 Mon Sep 17 00:00:00 2001 From: Lars Lorentz Ludvigsen Date: Sat, 14 May 2022 07:41:48 -0400 Subject: [PATCH 4/9] Updated README to match previous changes --- README.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index a84b86a..e726878 100644 --- a/README.md +++ b/README.md @@ -121,7 +121,9 @@ The `camera_pkg_launch.py` included in this package provides an example demonstr executable='camera_node', name='camera_node' parameters=[ - {'resize_images': 4}, + {'resize_images': True}, + {'resize_images_factor': 4}, + {'fps': 0}, {'display_topic_enable': True} ] ) @@ -132,7 +134,9 @@ The `camera_pkg_launch.py` included in this package provides an example demonstr | Parameter name | Description | | ---------------- | ----------- | -| `resize_images` | Set to positive integer depending on if you want to resize the images in camera_pkg. Use 1 for no resizing | +| `resize_images` | Set to `True` or `False` depending on if you want to resize the images in camera_pkg | +| `resize_images_factor` | Will downscale the image with a factor. Default is 4 to create 160 x 120 pixel image | +| `fps` | Will limit the number of pictures per second that are captured. Default value is 0 which will let camera control frames per second | | `display_topic_enable` | Set to true if you want to enable the `camera_pkg`/`display_mjpeg` topic | From fa187eb20fab7cd3e720a8cfc6f06982f80e06ae Mon Sep 17 00:00:00 2001 From: Lars Ludvigsen Date: Sat, 30 Jul 2022 13:47:24 +0000 Subject: [PATCH 5/9] Change timing --- camera_pkg/src/camera_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/camera_pkg/src/camera_node.cpp b/camera_pkg/src/camera_node.cpp index a3d2e6e..3e66e44 100644 --- a/camera_pkg/src/camera_node.cpp +++ b/camera_pkg/src/camera_node.cpp @@ -163,7 +163,6 @@ namespace MediaEng { while (produceFrames_) { deepracer_interfaces_pkg::msg::CameraMsg msg; std_msgs::msg::Header header; - header.stamp = this->get_clock()->now(); header.frame_id = std::to_string(imageFrameId_++); for (auto& cap : videoCaptureList_) { if (!cap.isOpened()) { @@ -171,6 +170,7 @@ namespace MediaEng { } cv::Mat frame; cap >> frame; + header.stamp = this->get_clock()->now(); if (frame.empty()) { RCLCPP_ERROR(this->get_logger(), "No frame returned. Check if camera is plugged in correctly."); continue; From de47408a7a72a3ba0fc75659f19019536d4fa282 Mon Sep 17 00:00:00 2001 From: Lars Ludvigsen Date: Tue, 30 Aug 2022 06:25:38 +0000 Subject: [PATCH 6/9] Fix FPS adjustor --- camera_pkg/src/camera_node.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/camera_pkg/src/camera_node.cpp b/camera_pkg/src/camera_node.cpp index 3e66e44..a8b1679 100644 --- a/camera_pkg/src/camera_node.cpp +++ b/camera_pkg/src/camera_node.cpp @@ -98,6 +98,13 @@ namespace MediaEng { for (auto idx : cameraIdxList){ RCLCPP_INFO(this->get_logger(), "Scanning Camera Index %d ", idx); auto cap = cv::VideoCapture(idx, cv::CAP_V4L); + // Set the FPS + if (framesPerSecond_ > 0 && cap.isOpened()) + { + RCLCPP_INFO(this->get_logger(), "[Camera Package] Changing from %d to %d fps on index %d.", + (int)cap.get(cv::CAP_PROP_FPS), framesPerSecond_.load(), idx); + cap.set(cv::CAP_PROP_FPS, framesPerSecond_.load()); + } cv::Mat test_frame; cap >> test_frame; if(test_frame.empty() || !cap.isOpened()){ @@ -105,14 +112,9 @@ namespace MediaEng { continue; } // Add to valid video capture list - videoCaptureList_.push_back(cap); + videoCaptureList_.push_back(cap); videoCaptureList_.back().set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); - // Set the FPS - if (framesPerSecond_ > 0) { - RCLCPP_INFO(this->get_logger(), "[Camera Package] Setting %d fps on index %d.", framesPerSecond_.load(), idx); - videoCaptureList_.back().set(cv::CAP_PROP_FPS, framesPerSecond_.load()); - } // Add to valid video index list videoIndexList_.push_back(idx); } From 8f484706f4b3c2b66043ff54cb970e84932d3c03 Mon Sep 17 00:00:00 2001 From: Lars Ludvigsen Date: Wed, 31 Aug 2022 17:02:14 -0400 Subject: [PATCH 7/9] Compressed image --- camera_pkg/src/camera_node.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/camera_pkg/src/camera_node.cpp b/camera_pkg/src/camera_node.cpp index a8b1679..5d6fdb5 100644 --- a/camera_pkg/src/camera_node.cpp +++ b/camera_pkg/src/camera_node.cpp @@ -16,6 +16,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" #include "cv_bridge/cv_bridge.h" #include "deepracer_interfaces_pkg/srv/video_state_srv.hpp" #include "deepracer_interfaces_pkg/msg/camera_msg.hpp" @@ -78,7 +79,7 @@ namespace MediaEng { // The queue size for displayPub_ is set to 10 because web_video_server subscribes to the camera_node and the // image callback is blocked since it probably expects to send a frame which has been lost due to small publisher queue size of 1 earlier. if (enableDisplayPub_) - displayPub_ = this->create_publisher(DISPLAY_MSG_TOPIC, 10); + displayPub_ = this->create_publisher(DISPLAY_MSG_TOPIC, 10); // Create a service to activate the publish of camera images. activateCameraService_ = this->create_service( @@ -181,7 +182,7 @@ namespace MediaEng { if(resizeImages_) { cv::resize(frame, frame, cv::Size((int) DEFAULT_IMAGE_WIDTH / resizeImagesFactor_, (int) DEFAULT_IMAGE_HEIGHT / resizeImagesFactor_)); } - msg.images.push_back(*(cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg().get())); + msg.images.push_back(*(cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toCompressedImageMsg().get())); } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); @@ -203,7 +204,7 @@ namespace MediaEng { /// ROS publisher object to the publish camera images to camera message topic. rclcpp::Publisher::SharedPtr videoPub_; /// ROS publisher object to the publish camera images to display message topic. - rclcpp::Publisher::SharedPtr displayPub_; + rclcpp::Publisher::SharedPtr displayPub_; /// ROS service object to activate the camera to publish images. rclcpp::Service::SharedPtr activateCameraService_; /// Boolean for starting and stopping the worker thread. From ece654661d13fabafba0c34976fe86c2044288c3 Mon Sep 17 00:00:00 2001 From: Lars Ludvigsen Date: Thu, 1 Sep 2022 21:48:40 +0000 Subject: [PATCH 8/9] Introducing image_transport --- camera_pkg/src/camera_node.cpp | 35 +++++++++++++++++++++++++--------- 1 file changed, 26 insertions(+), 9 deletions(-) diff --git a/camera_pkg/src/camera_node.cpp b/camera_pkg/src/camera_node.cpp index 5d6fdb5..3740686 100644 --- a/camera_pkg/src/camera_node.cpp +++ b/camera_pkg/src/camera_node.cpp @@ -21,6 +21,7 @@ #include "deepracer_interfaces_pkg/srv/video_state_srv.hpp" #include "deepracer_interfaces_pkg/msg/camera_msg.hpp" #include "opencv2/opencv.hpp" +#include "image_transport/image_transport.hpp" #include #include @@ -43,12 +44,14 @@ namespace MediaEng { /// indices where the camera is connected. CameraNode(const std::string & node_name, const std::vector cameraIdxList) : Node(node_name), + node_handle_(std::shared_ptr(this, [](auto *) {})), + image_transport_(node_handle_), produceFrames_(false), resizeImages_(true), resizeImagesFactor_(4), enableDisplayPub_(true), framesPerSecond_(0), - imageFrameId_(0) + imageFrameId_(0) { RCLCPP_INFO(this->get_logger(), "%s started", node_name.c_str()); @@ -78,8 +81,8 @@ namespace MediaEng { // This displays only the left/center camera images. Modify if the requirement is to publish images from both cameras. // The queue size for displayPub_ is set to 10 because web_video_server subscribes to the camera_node and the // image callback is blocked since it probably expects to send a frame which has been lost due to small publisher queue size of 1 earlier. - if (enableDisplayPub_) - displayPub_ = this->create_publisher(DISPLAY_MSG_TOPIC, 10); + if(enableDisplayPub_) + displayPub_ = image_transport_.advertise(DISPLAY_MSG_TOPIC, 10); // Create a service to activate the publish of camera images. activateCameraService_ = this->create_service( @@ -164,8 +167,10 @@ namespace MediaEng { /// to run in a separate thread. void produceFrames() { while (produceFrames_) { - deepracer_interfaces_pkg::msg::CameraMsg msg; + deepracer_interfaces_pkg::msg::CameraMsg cameraMsg; + sensor_msgs::msg::Image displayMsg; std_msgs::msg::Header header; + bool firstCamera = true; header.frame_id = std::to_string(imageFrameId_++); for (auto& cap : videoCaptureList_) { if (!cap.isOpened()) { @@ -182,7 +187,11 @@ namespace MediaEng { if(resizeImages_) { cv::resize(frame, frame, cv::Size((int) DEFAULT_IMAGE_WIDTH / resizeImagesFactor_, (int) DEFAULT_IMAGE_HEIGHT / resizeImagesFactor_)); } - msg.images.push_back(*(cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toCompressedImageMsg().get())); + if(enableDisplayPub_ && firstCamera){ + displayMsg = *(cv_bridge::CvImage(header, "bgr8", frame).toImageMsg().get()); + firstCamera = false; + } + cameraMsg.images.push_back(*(cv_bridge::CvImage(header, "bgr8", frame).toCompressedImageMsg().get())); } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); @@ -192,19 +201,24 @@ namespace MediaEng { } try { if (enableDisplayPub_) - displayPub_->publish(msg.images.front()); - videoPub_->publish(msg); + displayPub_.publish(displayMsg); + videoPub_->publish(cameraMsg); } catch (const std::exception &ex) { RCLCPP_ERROR(this->get_logger(), "Publishing camera images to topics failed %s", ex.what()); } + firstCamera = true; } } /// ROS publisher object to the publish camera images to camera message topic. rclcpp::Publisher::SharedPtr videoPub_; + /// Pointer to itself + rclcpp::Node::SharedPtr node_handle_; + /// Image transport object + image_transport::ImageTransport image_transport_; /// ROS publisher object to the publish camera images to display message topic. - rclcpp::Publisher::SharedPtr displayPub_; + image_transport::Publisher displayPub_; /// ROS service object to activate the camera to publish images. rclcpp::Service::SharedPtr activateCameraService_; /// Boolean for starting and stopping the worker thread. @@ -244,7 +258,10 @@ int main(int argc, char * argv[]) // In case of Stereo Cameras: The index with greater number represents Left Camera. std::vector cameraIndex {4, 3, 2, 1, 0}; // Create the camera_node. - rclcpp::spin(std::make_shared("camera_node", cameraIndex)); + auto node = std::make_shared("camera_node", cameraIndex); + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(node); + exec.spin(); rclcpp::shutdown(); return 0; } From 460f955b93a99084a85a84eaf6da5984c07abc51 Mon Sep 17 00:00:00 2001 From: Lars Ludvigsen Date: Fri, 28 Oct 2022 18:16:19 +0000 Subject: [PATCH 9/9] Fix dependency --- camera_pkg/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/camera_pkg/package.xml b/camera_pkg/package.xml index 132bf17..102ddb3 100644 --- a/camera_pkg/package.xml +++ b/camera_pkg/package.xml @@ -18,6 +18,7 @@ deepracer_interfaces_pkg cv_bridge image_transport + compressed_image_transport sensor_msgs std_msgs