diff --git a/pylon_ros2_camera_component/include/pylon_ros2_camera_node.hpp b/pylon_ros2_camera_component/include/pylon_ros2_camera_node.hpp index cb77e7f3..39b892c1 100644 --- a/pylon_ros2_camera_component/include/pylon_ros2_camera_node.hpp +++ b/pylon_ros2_camera_component/include/pylon_ros2_camera_node.hpp @@ -1628,7 +1628,7 @@ class PylonROS2CameraNode : public rclcpp::Node PylonROS2CameraParameter pylon_camera_parameter_set_; camera_info_manager::CameraInfoManager* camera_info_manager_; - sensor_msgs::msg::Image img_raw_msg_; + sensor_msgs::msg::Image::UniquePtr img_raw_msg_; cv_bridge::CvImage* cv_bridge_img_rect_; @@ -1656,6 +1656,7 @@ class PylonROS2CameraNode : public rclcpp::Node rclcpp::Publisher::SharedPtr blaze_depth_map_color_pub_; rclcpp::Publisher::SharedPtr blaze_confidence_pub_; rclcpp::Publisher::SharedPtr blaze_cam_info_pub_; + rclcpp::Publisher::SharedPtr ros_img_raw_pub_; // services rclcpp::Service::SharedPtr get_max_num_buffer_srv_; @@ -1803,7 +1804,7 @@ class PylonROS2CameraNode : public rclcpp::Node std::vector sampling_indices_; std::array brightness_exp_lut_; - bool is_sleeping_; + bool is_sleeping_, use_intra_process_comms_; // diagnostics diagnostic_updater::Updater diagnostics_updater_; diff --git a/pylon_ros2_camera_component/include/pylon_ros2_camera_parameter.hpp b/pylon_ros2_camera_component/include/pylon_ros2_camera_parameter.hpp index cc137b3e..d3dfe2da 100644 --- a/pylon_ros2_camera_component/include/pylon_ros2_camera_parameter.hpp +++ b/pylon_ros2_camera_component/include/pylon_ros2_camera_parameter.hpp @@ -305,6 +305,11 @@ class PylonROS2CameraParameter * Only supported for GigE cameras. Default: true */ bool auto_flash_line_3_; + + /** + * Flag that indicates if the camera should be sleeping on startup + */ + bool sleep_on_startup_; /** * camera grab timeout in ms diff --git a/pylon_ros2_camera_component/src/pylon_ros2_camera_node.cpp b/pylon_ros2_camera_component/src/pylon_ros2_camera_node.cpp index 49bd0dcb..d04ce433 100644 --- a/pylon_ros2_camera_component/src/pylon_ros2_camera_node.cpp +++ b/pylon_ros2_camera_component/src/pylon_ros2_camera_node.cpp @@ -53,6 +53,7 @@ PylonROS2CameraNode::PylonROS2CameraNode(const rclcpp::NodeOptions& options) , brightness_exp_lut_() , is_sleeping_(false) , diagnostics_updater_(this) + , use_intra_process_comms_(options.use_intra_process_comms()) { // information logging severity mode rcutils_ret_t __attribute__((unused)) res = rcutils_logging_set_logger_level(LOGGER.get_name(), RCUTILS_LOG_SEVERITY_DEBUG); @@ -69,6 +70,12 @@ PylonROS2CameraNode::PylonROS2CameraNode(const rclcpp::NodeOptions& options) if (!this->init()) return; + + if (this->pylon_camera_parameter_set_.sleep_on_startup_) + { + RCLCPP_INFO(LOGGER, "Camera is set to sleep on startup"); + } + this->is_sleeping_ = this->pylon_camera_parameter_set_.sleep_on_startup_; // starting spinning thread RCLCPP_INFO_STREAM(LOGGER, "Start image grabbing if node connects to topic with " << "a spinning rate of: " << this->frameRate() << " Hz"); timer_ = this->create_wall_timer( @@ -165,7 +172,15 @@ void PylonROS2CameraNode::initPublishers() this->component_status_pub_ = this->create_publisher(msg_name, 5); msg_name = msg_prefix + "image_raw"; - this->img_raw_pub_ = image_transport::create_camera_publisher(this, msg_name); + + if (this->use_intra_process_comms_) + { + this->ros_img_raw_pub_ = this->create_publisher(msg_name, 10); + } + else + { + this->img_raw_pub_ = image_transport::create_camera_publisher(this, msg_name); + } // blaze related topics msg_name = msg_prefix + "blaze_cloud"; this->blaze_cloud_topic_name_ = msg_name; @@ -701,16 +716,18 @@ bool PylonROS2CameraNode::startGrabbing() }); } } + this->img_raw_msg_.release(); + this->img_raw_msg_ = std::make_unique(); - this->img_raw_msg_.header.frame_id = this->pylon_camera_parameter_set_.cameraFrame(); + this->img_raw_msg_->header.frame_id = this->pylon_camera_parameter_set_.cameraFrame(); // Encoding of pixels -- channel meaning, ordering, size // taken from the list of strings in include/sensor_msgs/image_encodings.h - this->img_raw_msg_.encoding = this->pylon_camera_->currentROSEncoding(); - this->img_raw_msg_.height = this->pylon_camera_->imageRows(); - this->img_raw_msg_.width = this->pylon_camera_->imageCols(); + this->img_raw_msg_->encoding = this->pylon_camera_->currentROSEncoding(); + this->img_raw_msg_->height = this->pylon_camera_->imageRows(); + this->img_raw_msg_->width = this->pylon_camera_->imageCols(); // step = full row length in bytes, img_size = (step * rows), imagePixelDepth // already contains the number of channels - this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); if (!this->camera_info_manager_->setCameraName(this->pylon_camera_->deviceUserID())) { @@ -749,7 +766,7 @@ bool PylonROS2CameraNode::startGrabbing() this->setupRectification(); // set the correct tf frame_id sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); - cam_info.header.frame_id = this->img_raw_msg_.header.frame_id; + cam_info.header.frame_id = this->img_raw_msg_->header.frame_id; this->camera_info_manager_->setCameraInfo(cam_info); } else @@ -922,9 +939,9 @@ void PylonROS2CameraNode::spin() if (!this->pylon_camera_->isBlaze()) { - if (!this->isSleeping() && (this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub())) + if (!this->isSleeping() && (this->use_intra_process_comms_ || this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub())) { - if (this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub()) + if (this->use_intra_process_comms_ || this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub()) { if (!this->grabImage()) { @@ -932,26 +949,40 @@ void PylonROS2CameraNode::spin() } } - if (this->img_raw_pub_.getNumSubscribers() > 0) - { - // get actual cam_info-object in every frame, because it might have - // changed due to a 'set_camera_info'-service call - sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); - cam_info.header.stamp = this->img_raw_msg_.header.stamp; - // publish via image_transport - this->img_raw_pub_.publish(this->img_raw_msg_, cam_info); - } - - // this->getNumSubscribersRectImagePub() involves that this->camera_info_manager_->isCalibrated() == true if (this->getNumSubscribersRectImagePub() > 0) { - this->cv_bridge_img_rect_->header.stamp = this->img_raw_msg_.header.stamp; + this->cv_bridge_img_rect_->header.stamp = this->img_raw_msg_->header.stamp; assert(this->pinhole_model_->initialized()); - cv_bridge::CvImagePtr cv_img_raw = cv_bridge::toCvCopy(this->img_raw_msg_, this->img_raw_msg_.encoding); + cv_bridge::CvImagePtr cv_img_raw = cv_bridge::toCvCopy(*this->img_raw_msg_, this->img_raw_msg_->encoding); this->pinhole_model_->fromCameraInfo(this->camera_info_manager_->getCameraInfo()); this->pinhole_model_->rectifyImage(cv_img_raw->image, this->cv_bridge_img_rect_->image); this->img_rect_pub_->publish(this->cv_bridge_img_rect_->toImageMsg()); } + + if (this->use_intra_process_comms_ || this->img_raw_pub_.getNumSubscribers() > 0) + { + // get actual cam_info-object in every frame, because it might have + // changed due to a 'set_camera_info'-service call + sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); + cam_info.header.stamp = this->img_raw_msg_->header.stamp; + if (this->use_intra_process_comms_) + { + // publish directly + this->ros_img_raw_pub_->publish(std::move(this->img_raw_msg_)); + this->img_raw_msg_ = std::make_unique(); + this->img_raw_msg_->header.frame_id = this->pylon_camera_parameter_set_.cameraFrame(); + this->img_raw_msg_->encoding = this->pylon_camera_->currentROSEncoding(); + this->img_raw_msg_->height = this->pylon_camera_->imageRows(); + this->img_raw_msg_->width = this->pylon_camera_->imageCols(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); + } + else + { + // publish via image_transport + this->img_raw_pub_.publish(*this->img_raw_msg_, cam_info); + } + } + // this->getNumSubscribersRectImagePub() involves that this->camera_info_manager_->isCalibrated() == true } } else @@ -1016,11 +1047,11 @@ bool PylonROS2CameraNode::grabImage() { // Store current time before the image is transmitted for a more accurate grab time estimation auto grab_time = rclcpp::Node::now(); - if (!this->pylon_camera_->grab(this->img_raw_msg_.data)) + if (!this->pylon_camera_->grab(this->img_raw_msg_->data)) { return false; } - this->img_raw_msg_.header.stamp = grab_time; + this->img_raw_msg_->header.stamp = grab_time; } else { @@ -1430,11 +1461,11 @@ bool PylonROS2CameraNode::setROI(const sensor_msgs::msg::RegionOfInterest target sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); cam_info.roi = this->pylon_camera_->currentROI(); this->camera_info_manager_->setCameraInfo(cam_info); - this->img_raw_msg_.width = this->pylon_camera_->imageCols(); - this->img_raw_msg_.height = this->pylon_camera_->imageRows(); + this->img_raw_msg_->width = this->pylon_camera_->imageCols(); + this->img_raw_msg_->height = this->pylon_camera_->imageRows(); // step = full row length in bytes, img_size = (step * rows), imagePixelDepth // already contains the number of channels - this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); return false; } r.sleep(); @@ -1444,11 +1475,11 @@ bool PylonROS2CameraNode::setROI(const sensor_msgs::msg::RegionOfInterest target sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); cam_info.roi = this->pylon_camera_->currentROI(); this->camera_info_manager_->setCameraInfo(cam_info); - this->img_raw_msg_.height = this->pylon_camera_->imageRows(); - this->img_raw_msg_.width = this->pylon_camera_->imageCols(); + this->img_raw_msg_->height = this->pylon_camera_->imageRows(); + this->img_raw_msg_->width = this->pylon_camera_->imageCols(); // step = full row length in bytes, img_size = (step * rows), imagePixelDepth // already contains the number of channels - this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); this->setupSamplingIndices(this->sampling_indices_, this->pylon_camera_->imageRows(), this->pylon_camera_->imageCols(), @@ -1480,10 +1511,10 @@ bool PylonROS2CameraNode::setBinningX(const size_t& target_binning_x, sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); cam_info.binning_x = this->pylon_camera_->currentBinningX(); this->camera_info_manager_->setCameraInfo(cam_info); - this->img_raw_msg_.width = this->pylon_camera_->imageCols(); + this->img_raw_msg_->width = this->pylon_camera_->imageCols(); // step = full row length in bytes, img_size = (step * rows), imagePixelDepth // already contains the number of channels - this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); return false; } @@ -1494,10 +1525,10 @@ bool PylonROS2CameraNode::setBinningX(const size_t& target_binning_x, sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); cam_info.binning_x = this->pylon_camera_->currentBinningX(); this->camera_info_manager_->setCameraInfo(cam_info); - this->img_raw_msg_.width = this->pylon_camera_->imageCols(); + this->img_raw_msg_->width = this->pylon_camera_->imageCols(); // step = full row length in bytes, img_size = (step * rows), imagePixelDepth // already contains the number of channels - this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); this->setupSamplingIndices(this->sampling_indices_, this->pylon_camera_->imageRows(), this->pylon_camera_->imageCols(), @@ -1528,10 +1559,10 @@ bool PylonROS2CameraNode::setBinningY(const size_t& target_binning_y, sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); cam_info.binning_y = this->pylon_camera_->currentBinningY(); this->camera_info_manager_->setCameraInfo(cam_info); - this->img_raw_msg_.height = this->pylon_camera_->imageRows(); + this->img_raw_msg_->height = this->pylon_camera_->imageRows(); // step = full row length in bytes, img_size = (step * rows), imagePixelDepth // already contains the number of channels - this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); return false; } r.sleep(); @@ -1541,10 +1572,10 @@ bool PylonROS2CameraNode::setBinningY(const size_t& target_binning_y, sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo(); cam_info.binning_y = this->pylon_camera_->currentBinningY(); this->camera_info_manager_->setCameraInfo(cam_info); - this->img_raw_msg_.height = this->pylon_camera_->imageRows(); + this->img_raw_msg_->height = this->pylon_camera_->imageRows(); // step = full row length in bytes, img_size = (step * rows), imagePixelDepth // already contains the number of channels - this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth(); + this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth(); this->setupSamplingIndices(this->sampling_indices_, this->pylon_camera_->imageRows(), this->pylon_camera_->imageCols(), @@ -4579,16 +4610,16 @@ void PylonROS2CameraNode::genSamplingIndicesRec(std::vector& indice float PylonROS2CameraNode::calcCurrentBrightness() { std::lock_guard lock(this->grab_mutex_); - if (this->img_raw_msg_.data.empty()) + if (this->img_raw_msg_->data.empty()) { return 0.0; } float sum = 0.0; - if (sensor_msgs::image_encodings::isMono(this->img_raw_msg_.encoding)) + if (sensor_msgs::image_encodings::isMono(this->img_raw_msg_->encoding)) { // Check if image is expected size so indices don't fall out of bounds - if (this->img_raw_msg_.data.size() != this->img_raw_msg_.height * this->img_raw_msg_.width) + if (this->img_raw_msg_->data.size() != this->img_raw_msg_->height * this->img_raw_msg_->width) { return 0.0; } @@ -4596,7 +4627,7 @@ float PylonROS2CameraNode::calcCurrentBrightness() // The mean brightness is calculated using a subset of all pixels for (const std::size_t& idx : this->sampling_indices_) { - sum += this->img_raw_msg_.data.at(idx); + sum += this->img_raw_msg_->data.at(idx); } if (sum > 0.0) { @@ -4606,10 +4637,10 @@ float PylonROS2CameraNode::calcCurrentBrightness() else { // The mean brightness is calculated using all pixels and all channels - sum = std::accumulate(this->img_raw_msg_.data.begin(), this->img_raw_msg_.data.end(), 0); + sum = std::accumulate(this->img_raw_msg_->data.begin(), this->img_raw_msg_->data.end(), 0); if (sum > 0.0) { - sum /= static_cast(this->img_raw_msg_.data.size()); + sum /= static_cast(this->img_raw_msg_->data.size()); } } @@ -4659,8 +4690,8 @@ void PylonROS2CameraNode::setupRectification() { this->cv_bridge_img_rect_ = new cv_bridge::CvImage(); } - this->cv_bridge_img_rect_->header = img_raw_msg_.header; - this->cv_bridge_img_rect_->encoding = img_raw_msg_.encoding; + this->cv_bridge_img_rect_->header = img_raw_msg_->header; + this->cv_bridge_img_rect_->encoding = img_raw_msg_->encoding; } std::shared_ptr PylonROS2CameraNode::grabRawImages(const std::shared_ptr goal_handle) diff --git a/pylon_ros2_camera_component/src/pylon_ros2_camera_parameter.cpp b/pylon_ros2_camera_component/src/pylon_ros2_camera_parameter.cpp index dc5296ee..1aadbde7 100644 --- a/pylon_ros2_camera_component/src/pylon_ros2_camera_parameter.cpp +++ b/pylon_ros2_camera_component/src/pylon_ros2_camera_parameter.cpp @@ -405,6 +405,12 @@ void PylonROS2CameraParameter::readFromRosParameterServer(rclcpp::Node& nh) } nh.get_parameter("auto_flash_line_3", this->auto_flash_line_3_); + if (!nh.has_parameter("sleep_on_startup")) + { + nh.declare_parameter("sleep_on_startup", false); + } + nh.get_parameter("sleep_on_startup", this->sleep_on_startup_); + RCLCPP_WARN(LOGGER, "Autoflash: %i, line2: %i, line3: %i", this->auto_flash_, this->auto_flash_line_2_, this->auto_flash_line_3_); this->validateParameterSet(nh);