From 76f55e7e13a2170e58b8451a5ca24c3aca0c16c1 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 21 Aug 2022 01:03:53 +0100 Subject: [PATCH 01/32] moved headers to include --- {src => include}/camerainfolist.h | 0 {src => include}/genicam_camera_nodelet.h | 0 {src => include}/imagelist.h | 0 {src => include}/publishers/camera_info_publisher.h | 0 {src => include}/publishers/image_publisher.h | 0 {src => include}/timestamp_corrector.h | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename {src => include}/camerainfolist.h (100%) rename {src => include}/genicam_camera_nodelet.h (100%) rename {src => include}/imagelist.h (100%) rename {src => include}/publishers/camera_info_publisher.h (100%) rename {src => include}/publishers/image_publisher.h (100%) rename {src => include}/timestamp_corrector.h (100%) diff --git a/src/camerainfolist.h b/include/camerainfolist.h similarity index 100% rename from src/camerainfolist.h rename to include/camerainfolist.h diff --git a/src/genicam_camera_nodelet.h b/include/genicam_camera_nodelet.h similarity index 100% rename from src/genicam_camera_nodelet.h rename to include/genicam_camera_nodelet.h diff --git a/src/imagelist.h b/include/imagelist.h similarity index 100% rename from src/imagelist.h rename to include/imagelist.h diff --git a/src/publishers/camera_info_publisher.h b/include/publishers/camera_info_publisher.h similarity index 100% rename from src/publishers/camera_info_publisher.h rename to include/publishers/camera_info_publisher.h diff --git a/src/publishers/image_publisher.h b/include/publishers/image_publisher.h similarity index 100% rename from src/publishers/image_publisher.h rename to include/publishers/image_publisher.h diff --git a/src/timestamp_corrector.h b/include/timestamp_corrector.h similarity index 100% rename from src/timestamp_corrector.h rename to include/timestamp_corrector.h From 56345048e12dc8cbee6246c5ad2ccb50306b9c53 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 21 Aug 2022 19:57:08 +0100 Subject: [PATCH 02/32] infolist modified --- include/camerainfolist.h | 13 +++++++------ src/camerainfolist.cc | 16 ++++++++-------- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/include/camerainfolist.h b/include/camerainfolist.h index a3fef2d..4f09ed6 100644 --- a/include/camerainfolist.h +++ b/include/camerainfolist.h @@ -36,8 +36,9 @@ #ifndef RC_GENICAM_CAMERA_CAMERAINFOLIST #define RC_GENICAM_CAMERA_CAMERAINFOLIST -#include -#include +#include +#include +#include namespace rcgccam { @@ -72,7 +73,7 @@ class CameraInfoList @return Dropped camera info message, null pointer if nothing is dropped. */ - sensor_msgs::CameraInfoPtr add(const sensor_msgs::CameraInfoPtr& info); + sensor_msgs::msg::CameraInfo add(const sensor_msgs::msg::CameraInfo& info); /** Remove all camera infos that have a timestamp that is older or equal than @@ -81,7 +82,7 @@ class CameraInfoList @param timestamp Timestamp. @return Number of removed camera infos. */ - int removeOld(const ros::Time& timestamp); + int removeOld(const rclcpp::Time& timestamp); /** Returns the oldest camera info that has a timestamp within the tolerance @@ -92,12 +93,12 @@ class CameraInfoList @param tolerance Maximum tolarance added or subtracted to the timestamp. @return Pointer to camera info or 0. */ - sensor_msgs::CameraInfoPtr find(const ros::Time& timestamp) const; + sensor_msgs::msg::CameraInfo find(const rclcpp::Time& timestamp) const; private: size_t maxsize_; uint64_t tolerance_; - std::vector list_; + std::vector list_; }; } // namespace rcgccam diff --git a/src/camerainfolist.cc b/src/camerainfolist.cc index a4ad1e1..74b88cd 100644 --- a/src/camerainfolist.cc +++ b/src/camerainfolist.cc @@ -54,11 +54,11 @@ void CameraInfoList::setTolerance(uint64_t tolerance) tolerance_ = tolerance; } -sensor_msgs::CameraInfoPtr CameraInfoList::add(const sensor_msgs::CameraInfoPtr& image) +sensor_msgs::msg::CameraInfo CameraInfoList::add(const sensor_msgs::msg::CameraInfo& image) { list_.push_back(image); - sensor_msgs::CameraInfoPtr ret; + sensor_msgs::msg::CameraInfo ret; if (list_.size() > maxsize_) { @@ -69,14 +69,14 @@ sensor_msgs::CameraInfoPtr CameraInfoList::add(const sensor_msgs::CameraInfoPtr& return ret; } -int CameraInfoList::removeOld(const ros::Time& timestamp) +int CameraInfoList::removeOld(const rclcpp::Time& timestamp) { size_t i = 0; int n = 0; while (i < list_.size()) { - if (list_[i]->header.stamp <= timestamp) + if (rclcpp::Time(list_[i].header.stamp) <= timestamp) { list_.erase(list_.begin() + static_cast(i)); n++; @@ -90,12 +90,12 @@ int CameraInfoList::removeOld(const ros::Time& timestamp) return n; } -sensor_msgs::CameraInfoPtr CameraInfoList::find(const ros::Time& timestamp) const +sensor_msgs::msg::CameraInfo CameraInfoList::find(const rclcpp::Time& timestamp) const { for (size_t i = 0; i < list_.size(); i++) { - uint64_t ts = timestamp.toNSec(); - uint64_t info_ts = list_[i]->header.stamp.toNSec(); + uint64_t ts = timestamp.nanoseconds(); + uint64_t info_ts = list_[i].header.stamp.nanosec; if (info_ts >= ts - tolerance_ && info_ts <= ts + tolerance_) { @@ -103,7 +103,7 @@ sensor_msgs::CameraInfoPtr CameraInfoList::find(const ros::Time& timestamp) cons } } - return sensor_msgs::CameraInfoPtr(); + return sensor_msgs::msg::CameraInfo(); } } // namespace rcgccam From 9f4732280fcccb15f9f3cb7323786104bc086adb Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 21 Aug 2022 20:45:37 +0100 Subject: [PATCH 03/32] modified with shared pointers --- include/camerainfolist.h | 6 +++--- src/camerainfolist.cc | 14 +++++++------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/camerainfolist.h b/include/camerainfolist.h index 4f09ed6..60194aa 100644 --- a/include/camerainfolist.h +++ b/include/camerainfolist.h @@ -73,7 +73,7 @@ class CameraInfoList @return Dropped camera info message, null pointer if nothing is dropped. */ - sensor_msgs::msg::CameraInfo add(const sensor_msgs::msg::CameraInfo& info); + sensor_msgs::msg::CameraInfo::ConstSharedPtr add(sensor_msgs::msg::CameraInfo::ConstSharedPtr info); /** Remove all camera infos that have a timestamp that is older or equal than @@ -93,12 +93,12 @@ class CameraInfoList @param tolerance Maximum tolarance added or subtracted to the timestamp. @return Pointer to camera info or 0. */ - sensor_msgs::msg::CameraInfo find(const rclcpp::Time& timestamp) const; + sensor_msgs::msg::CameraInfo::ConstSharedPtr find(const rclcpp::Time& timestamp) const; private: size_t maxsize_; uint64_t tolerance_; - std::vector list_; + std::vector list_; }; } // namespace rcgccam diff --git a/src/camerainfolist.cc b/src/camerainfolist.cc index 74b88cd..b8269c9 100644 --- a/src/camerainfolist.cc +++ b/src/camerainfolist.cc @@ -54,11 +54,11 @@ void CameraInfoList::setTolerance(uint64_t tolerance) tolerance_ = tolerance; } -sensor_msgs::msg::CameraInfo CameraInfoList::add(const sensor_msgs::msg::CameraInfo& image) +sensor_msgs::msg::CameraInfo::ConstSharedPtr CameraInfoList::add(sensor_msgs::msg::CameraInfo::ConstSharedPtr info) { - list_.push_back(image); + list_.push_back(info); - sensor_msgs::msg::CameraInfo ret; + sensor_msgs::msg::CameraInfo::ConstSharedPtr ret; if (list_.size() > maxsize_) { @@ -76,7 +76,7 @@ int CameraInfoList::removeOld(const rclcpp::Time& timestamp) while (i < list_.size()) { - if (rclcpp::Time(list_[i].header.stamp) <= timestamp) + if (rclcpp::Time(list_[i]->header.stamp) <= timestamp) { list_.erase(list_.begin() + static_cast(i)); n++; @@ -90,12 +90,12 @@ int CameraInfoList::removeOld(const rclcpp::Time& timestamp) return n; } -sensor_msgs::msg::CameraInfo CameraInfoList::find(const rclcpp::Time& timestamp) const +sensor_msgs::msg::CameraInfo::ConstSharedPtr CameraInfoList::find(const rclcpp::Time& timestamp) const { for (size_t i = 0; i < list_.size(); i++) { uint64_t ts = timestamp.nanoseconds(); - uint64_t info_ts = list_[i].header.stamp.nanosec; + uint64_t info_ts = list_[i]->header.stamp.nanosec; if (info_ts >= ts - tolerance_ && info_ts <= ts + tolerance_) { @@ -103,7 +103,7 @@ sensor_msgs::msg::CameraInfo CameraInfoList::find(const rclcpp::Time& timestamp) } } - return sensor_msgs::msg::CameraInfo(); + return sensor_msgs::msg::CameraInfo::ConstSharedPtr(); } } // namespace rcgccam From d2bea1777ec5f8b9e71a18a75c693bfa809cbb8b Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 21 Aug 2022 22:58:00 +0100 Subject: [PATCH 04/32] updated imagelist for ros2 --- include/imagelist.h | 12 ++++++------ src/imagelist.cc | 16 ++++++++-------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/include/imagelist.h b/include/imagelist.h index efa2118..095fd82 100644 --- a/include/imagelist.h +++ b/include/imagelist.h @@ -36,8 +36,8 @@ #ifndef RC_GENICAM_CAMERA_IMAGELIST #define RC_GENICAM_CAMERA_IMAGELIST -#include -#include +#include +#include namespace rcgccam { @@ -71,7 +71,7 @@ class ImageList @param image Image to be added. @return Dropped image, null pointer if no image is dropped. */ - sensor_msgs::ImagePtr add(const sensor_msgs::ImagePtr& image); + sensor_msgs::msg::Image::ConstSharedPtr add(sensor_msgs::msg::Image::ConstSharedPtr image); /** Remove all images that have a timestamp that is older or equal than the @@ -80,7 +80,7 @@ class ImageList @param timestamp Timestamp. @return Number of removed images. */ - int removeOld(const ros::Time& timestamp); + int removeOld(const rclcpp::Time& timestamp); /** Returns the oldest image that has a timestamp within the tolerance of the @@ -91,12 +91,12 @@ class ImageList @param tolerance Maximum tolarance added or subtracted to the timestamp. @return Pointer to image or 0. */ - sensor_msgs::ImagePtr find(const ros::Time& timestamp) const; + sensor_msgs::msg::Image::ConstSharedPtr find(const rclcpp::Time& timestamp) const; private: size_t maxsize_; uint64_t tolerance_; - std::vector list_; + std::vector list_; }; } // namespace rcgccam diff --git a/src/imagelist.cc b/src/imagelist.cc index 404b7c1..b2bf901 100644 --- a/src/imagelist.cc +++ b/src/imagelist.cc @@ -54,11 +54,11 @@ void ImageList::setTolerance(uint64_t tolerance) tolerance_ = tolerance; } -sensor_msgs::ImagePtr ImageList::add(const sensor_msgs::ImagePtr& image) +sensor_msgs::msg::Image::ConstSharedPtr ImageList::add(sensor_msgs::msg::Image::ConstSharedPtr image) { list_.push_back(image); - sensor_msgs::ImagePtr ret; + sensor_msgs::msg::Image::ConstSharedPtr ret; if (list_.size() > maxsize_) { @@ -69,14 +69,14 @@ sensor_msgs::ImagePtr ImageList::add(const sensor_msgs::ImagePtr& image) return ret; } -int ImageList::removeOld(const ros::Time& timestamp) +int ImageList::removeOld(const rclcpp::Time& timestamp) { size_t i = 0; int n = 0; while (i < list_.size()) { - if (list_[i]->header.stamp <= timestamp) + if (rclcpp::Time(list_[i]->header.stamp) <= timestamp) { list_.erase(list_.begin() + static_cast(i)); n++; @@ -90,12 +90,12 @@ int ImageList::removeOld(const ros::Time& timestamp) return n; } -sensor_msgs::ImagePtr ImageList::find(const ros::Time& timestamp) const +sensor_msgs::msg::Image::ConstSharedPtr ImageList::find(const rclcpp::Time& timestamp) const { for (size_t i = 0; i < list_.size(); i++) { - uint64_t ts = timestamp.toNSec(); - uint64_t image_ts = list_[i]->header.stamp.toNSec(); + uint64_t ts = timestamp.nanoseconds(); + uint64_t image_ts = list_[i]->header.stamp.nanosec; if (image_ts >= ts - tolerance_ && image_ts <= ts + tolerance_) { @@ -103,7 +103,7 @@ sensor_msgs::ImagePtr ImageList::find(const ros::Time& timestamp) const } } - return sensor_msgs::ImagePtr(); + return sensor_msgs::msg::Image::ConstSharedPtr(); } } // namespace rcgccam From 3907a622ea78f287d541d4f82cc02b9d1e41dd1c Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 21 Aug 2022 23:06:04 +0100 Subject: [PATCH 05/32] tmestamp corrector updated for ros2 --- include/timestamp_corrector.h | 4 ++-- src/timestamp_corrector.cc | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/timestamp_corrector.h b/include/timestamp_corrector.h index b21a614..a824e19 100644 --- a/include/timestamp_corrector.h +++ b/include/timestamp_corrector.h @@ -34,7 +34,7 @@ #ifndef RC_GENICAM_CAMERA_TIMESTAMP_CORRECTOR_H #define RC_GENICAM_CAMERA_TIMESTAMP_CORRECTOR_H -#include +#include #include @@ -90,7 +90,7 @@ class TimestampCorrector determineOffset() has never been called or if it delivered an error on the last call. */ - int64_t correct(ros::Time& time); + int64_t correct(rclcpp::Time& time); private: int64_t tolerance_; diff --git a/src/timestamp_corrector.cc b/src/timestamp_corrector.cc index 4fdaecf..acf4d40 100644 --- a/src/timestamp_corrector.cc +++ b/src/timestamp_corrector.cc @@ -132,12 +132,12 @@ bool TimestampCorrector::determineOffset(const std::shared_ptr= 0 && accuracy_ >= 0) { - int64_t t = static_cast(time.toNSec()); - time.fromNSec(static_cast(t + offset_)); + int64_t t = static_cast(time.nanoseconds()); + time = rclcpp::Time(static_cast(t + offset_)); return accuracy_; } From 1f1922851e8be165a6930e98b89b1b6bf2abb4ef Mon Sep 17 00:00:00 2001 From: MRo47 Date: Mon, 22 Aug 2022 00:12:18 +0100 Subject: [PATCH 06/32] updated package xml for ros2 --- package.xml | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/package.xml b/package.xml index e8bc166..0bcbf92 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,6 @@ - + + rc_genicam_camera 1.3.0 @@ -15,25 +16,29 @@ rc_genicam_api - message_generation - message_runtime + ament_cmake - catkin - - nodelet - roscpp + builtin_interfaces + rosidl_default_generators + builtin_interfaces + rosidl_default_runtime + + + rclcpp std_srvs std_msgs sensor_msgs image_transport - dynamic_reconfigure - diagnostic_updater + + + + rosidl_interface_packages doxygen - + ament_cmake From ec725fca0ea86a275338d0cb9dde883298deebdc Mon Sep 17 00:00:00 2001 From: MRo47 Date: Tue, 23 Aug 2022 00:50:44 +0100 Subject: [PATCH 07/32] modifying node for ros2 --- ...camera_nodelet.h => genicam_camera_node.h} | 31 +++-- src/genicam_camera_nodelet.cc | 108 ++++++++++-------- 2 files changed, 75 insertions(+), 64 deletions(-) rename include/{genicam_camera_nodelet.h => genicam_camera_node.h} (71%) diff --git a/include/genicam_camera_nodelet.h b/include/genicam_camera_node.h similarity index 71% rename from include/genicam_camera_nodelet.h rename to include/genicam_camera_node.h index 55f7fe1..8417848 100644 --- a/include/genicam_camera_nodelet.h +++ b/include/genicam_camera_node.h @@ -39,9 +39,9 @@ #include "publishers/camera_info_publisher.h" #include "publishers/image_publisher.h" -#include +#include #include -#include +#include #include #include @@ -50,29 +50,28 @@ #include #include -#include -#include +#include +#include namespace rcgccam { -class GenICamCameraNodelet : public nodelet::Nodelet +class GenICamCameraNode : public rclcpp::Node { public: - - GenICamCameraNodelet(); - virtual ~GenICamCameraNodelet(); + explicit GenICamCameraNode(const std::string& node_name = "rc_genicam_camera_node"); + virtual ~GenICamCameraNode(); virtual void onInit(); - bool getGenICamParameter(rc_genicam_camera::GetGenICamParameter::Request& req, - rc_genicam_camera::GetGenICamParameter::Response& resp); + bool getGenICamParameter(rc_genicam_camera::srv::GetGenICamParameter::Request& req, + rc_genicam_camera::srv::GetGenICamParameter::Response& resp); - bool setGenICamParameter(rc_genicam_camera::SetGenICamParameter::Request& req, - rc_genicam_camera::SetGenICamParameter::Response& resp); + bool setGenICamParameter(rc_genicam_camera::srv::SetGenICamParameter::Request& req, + rc_genicam_camera::srv::SetGenICamParameter::Response& resp); - void syncInfo(sensor_msgs::CameraInfoPtr info); + void syncInfo(sensor_msgs::msg::CameraInfo::ConstSharedPtr info); private: @@ -81,10 +80,10 @@ class GenICamCameraNodelet : public nodelet::Nodelet double timestamp_tolerance_; double sync_tolerance_; - ros::Subscriber sub_sync_info_; + rclcpp::Subscription::SharedPtr sub_sync_info_ptr_; - ros::ServiceServer get_param_service_; - ros::ServiceServer set_param_service_; + rclcpp::Service::SharedPtr get_param_service_ptr_; + rclcpp::Service::SharedPtr set_param_service_ptr_; std::string frame_id_; diff --git a/src/genicam_camera_nodelet.cc b/src/genicam_camera_nodelet.cc index 37df229..6783c40 100644 --- a/src/genicam_camera_nodelet.cc +++ b/src/genicam_camera_nodelet.cc @@ -31,7 +31,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "genicam_camera_nodelet.h" +#include "genicam_camera_node.h" #include "timestamp_corrector.h" #include @@ -48,24 +48,39 @@ #include #include -#include -#include +#include +#include namespace rcgccam { +// TODO check this #define ROS_HAS_STEADYTIME (ROS_VERSION_MINIMUM(1, 13, 1) || ((ROS_VERSION_MINOR == 12) && ROS_VERSION_PATCH >= 8)) -GenICamCameraNodelet::GenICamCameraNodelet() +explicit GenICamCameraNode::GenICamCameraNode(const std::string& node_name) +: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) { timestamp_tolerance_ = 0; sync_tolerance_ = 0; rotate_ = false; running_ = false; + + this->declare_parameter("frame_id"); + this->declare_parameter("device"); + this->declare_parameter("gev_access"); + this->declare_parameter("config_file"); + this->declare_parameter("calib_file"); + this->declare_parameter("calib_id"); + this->declare_parameter("host_timestamp"); + this->declare_parameter("timestamp_tolerance_"); + this->declare_parameter("sync_info"); + this->declare_parameter("sync_tolerance"); + this->declare_parameter("image_prefix"); + this->declare_parameter("rotate"); } -GenICamCameraNodelet::~GenICamCameraNodelet() +GenICamCameraNode::~GenICamCameraNode() { - ROS_INFO("rc_genicam_camera: Shutting down"); + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Shutting down"); // signal running_ threads and wait until they finish @@ -78,26 +93,23 @@ GenICamCameraNodelet::~GenICamCameraNodelet() rcg::System::clearSystems(); } -void GenICamCameraNodelet::onInit() +void GenICamCameraNode::onInit() { - ROS_INFO("rc_genicam_camera: Initialization"); + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Initialization"); // get parameter configuration - ros::NodeHandle pnh(getPrivateNodeHandle()); - ros::NodeHandle nh(getNodeHandle(), ""); - std::string device = ""; std::string access = "control"; std::string config = ""; std::string calib = ""; int calib_id=-1; - pnh.param("frame_id", frame_id_, frame_id_); + frame_id_ = this->get_parameter("frame_id").as_string(); if (frame_id_.size() == 0) { - std::string ns = ros::this_node::getNamespace(); + std::string ns = this->get_namespace(); if (ns.size() > 0 && ns[0] == '/') { @@ -114,7 +126,7 @@ void GenICamCameraNodelet::onInit() } int cid=-1; - pnh.param("calib_id", cid, cid); + cid = this->get_parameter("calib_id").as_int(); if (cid >= 0) { @@ -122,15 +134,15 @@ void GenICamCameraNodelet::onInit() } } - pnh.param("device", device, device); - pnh.param("gev_access", access, access); - pnh.param("config_file", config, config); - pnh.param("calib_file", calib, calib); - pnh.param("calib_id", calib_id, calib_id); + device = this->get_parameter("device").as_string(); + access = this->get_parameter("gev_access").as_string(); + config = this->get_parameter("config_file").as_string(); + calib = this->get_parameter("calib_file").as_string(); + calib_id = this->get_parameter("calib_id").as_int(); if (device.size() == 0) { - ROS_FATAL("The GenICam device ID must be given in the private parameter 'device'!"); + RCLCPP_FATAL(this->get_logger(), "The GenICam device ID must be given in the private parameter 'device'!"); return; } @@ -145,7 +157,7 @@ void GenICamCameraNodelet::onInit() } else { - ROS_FATAL_STREAM("rc_visard_driver: Access must be 'control' or 'exclusive': " << access); + RCLCPP_FATAL_STREAM(this->get_logger(), "rc_visard_driver: Access must be 'control' or 'exclusive': " << access); return; } @@ -168,7 +180,7 @@ void GenICamCameraNodelet::onInit() if (sync_info.size() > 0) { - sub_sync_info_ = nh.subscribe(sync_info, 10, &GenICamCameraNodelet::syncInfo, this); + sub_sync_info_ = nh.subscribe(sync_info, 10, &GenICamCameraNode::syncInfo, this); image_list_.setSize(25); image_list_.setTolerance(static_cast(sync_tolerance_ * 1000000000.0)); @@ -183,9 +195,9 @@ void GenICamCameraNodelet::onInit() // setup service for getting and setting parameters - get_param_service_ = pnh.advertiseService("get_genicam_parameter", &GenICamCameraNodelet::getGenICamParameter, this); + get_param_service_ = pnh.advertiseService("get_genicam_parameter", &GenICamCameraNode::getGenICamParameter, this); - set_param_service_ = pnh.advertiseService("set_genicam_parameter", &GenICamCameraNodelet::setGenICamParameter, this); + set_param_service_ = pnh.advertiseService("set_genicam_parameter", &GenICamCameraNode::setGenICamParameter, this); // initialize publishers @@ -205,7 +217,7 @@ void GenICamCameraNodelet::onInit() // start grabbing threads running_ = true; - grab_thread_ = std::thread(&GenICamCameraNodelet::grab, this, device, access_id, config); + grab_thread_ = std::thread(&GenICamCameraNode::grab, this, device, access_id, config); } namespace @@ -226,7 +238,7 @@ std::string loadConfig(const std::string& filename) } else { - ROS_ERROR_STREAM("rc_genicam_camera: Cannot load config: " << filename); + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot load config: " << filename); } } @@ -287,7 +299,7 @@ void applyParameters(const std::shared_ptr& nodemap, const } // namespace -bool GenICamCameraNodelet::getGenICamParameter(rc_genicam_camera::GetGenICamParameter::Request& req, +bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::GetGenICamParameter::Request& req, rc_genicam_camera::GetGenICamParameter::Response& resp) { std::lock_guard lock(device_mtx_); @@ -302,7 +314,7 @@ bool GenICamCameraNodelet::getGenICamParameter(rc_genicam_camera::GetGenICamPara } catch (const std::exception& ex) { - ROS_ERROR_STREAM("rc_genicam_camera: Cannot get parameter: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot get parameter: " << ex.what()); resp.return_code.value = resp.return_code.INVALID_ARGUMENT; resp.return_code.message = ex.what(); @@ -312,7 +324,7 @@ bool GenICamCameraNodelet::getGenICamParameter(rc_genicam_camera::GetGenICamPara return true; } -bool GenICamCameraNodelet::setGenICamParameter(rc_genicam_camera::SetGenICamParameter::Request& req, +bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera::SetGenICamParameter::Request& req, rc_genicam_camera::SetGenICamParameter::Response& resp) { std::lock_guard lock(device_mtx_); @@ -328,7 +340,7 @@ bool GenICamCameraNodelet::setGenICamParameter(rc_genicam_camera::SetGenICamPara } catch (const std::exception& ex) { - ROS_ERROR_STREAM("rc_genicam_camera: Cannot set parameters: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot set parameters: " << ex.what()); resp.return_code.value = resp.return_code.INVALID_ARGUMENT; resp.return_code.message = ex.what(); @@ -373,7 +385,7 @@ void storeImage(const std::string &prefix, const sensor_msgs::ImagePtr &image) if (n < width*height) { - ROS_ERROR_STREAM("Cannot write to file " << name.str() << + RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot write to file " << name.str() << " (" << n << " < " << width*height << ")"); } @@ -381,14 +393,14 @@ void storeImage(const std::string &prefix, const sensor_msgs::ImagePtr &image) } else { - ROS_ERROR_STREAM("Cannot create file " << name.str()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot create file " << name.str()); } } } } -void GenICamCameraNodelet::syncInfo(sensor_msgs::CameraInfoPtr info) +void GenICamCameraNode::syncInfo(sensor_msgs::CameraInfoPtr info) { sensor_msgs::ImagePtr image; @@ -408,7 +420,7 @@ void GenICamCameraNodelet::syncInfo(sensor_msgs::CameraInfoPtr info) if (n > 0) { - ROS_WARN_STREAM("rc_genicam_camera: Dropped unused images: " << n); + RCLCPP_WARN_STREAM(this->get_logger(),"rc_genicam_camera: Dropped unused images: " << n); } // correct time stamp of image @@ -439,11 +451,11 @@ void GenICamCameraNodelet::syncInfo(sensor_msgs::CameraInfoPtr info) } } -void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, std::string config) +void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std::string config) { try { - ROS_INFO("rc_genicam_camera: Grabbing thread started"); + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread started"); // load initial configuration for camera into string @@ -486,7 +498,7 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, } catch (const std::exception& ex) { - ROS_ERROR_STREAM("rc_genicam_camera: Error during initial camera configuration: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Error during initial camera configuration: " << ex.what()); } } @@ -494,7 +506,7 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, if (!ts_host.determineOffset(rcgnodemap_)) { - ROS_ERROR_STREAM( + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot determine offset between host and camera clock with maximum tolerance of " << timestamp_tolerance_ << " s"); } @@ -558,7 +570,7 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, if (n > 0) { - ROS_WARN_STREAM("rc_genicam_camera: Dropped images: " << n); + RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Dropped images: " << n); } // correct time stamp of image @@ -574,7 +586,7 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, if (image) { - ROS_WARN_THROTTLE(10, "rc_genicam_camera: Input queue full, dropping image"); + RCLCPP_WARN_THROTTLE(this->get_logger(), 10, "rc_genicam_camera: Input queue full, dropping image"); } image.reset(); @@ -598,7 +610,7 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, } else { - ROS_ERROR_STREAM("rc_genicam_camera: Unsupported pixel format"); + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Unsupported pixel format"); } } } @@ -607,14 +619,14 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, if (!ts_host.determineOffset(rcgnodemap_)) { - ROS_ERROR_STREAM("rc_genicam_camera: Cannot determine offset between host and camera clock with " + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot determine offset between host and camera clock with " "maximum tolerance of " << timestamp_tolerance_ << " s"); } } else { - ROS_WARN_STREAM("rc_genicam_camera: Incomplete buffer received"); + RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Incomplete buffer received"); } } } @@ -626,7 +638,7 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, } catch (const std::exception& ex) { - ROS_ERROR_STREAM("rc_genicam_camera: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); sleep(1); } @@ -645,17 +657,17 @@ void GenICamCameraNodelet::grab(std::string device, rcg::Device::ACCESS access, } catch (const std::exception& ex) { - ROS_FATAL_STREAM("rc_genicam_camera: " << ex.what()); + RCLCPP_FATAL_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); } catch (...) { - ROS_FATAL_STREAM("rc_genicam_camera: Unknown exception"); + RCLCPP_FATAL_STREAM(this->get_logger(),"rc_genicam_camera: Unknown exception"); } running_ = false; - ROS_INFO("rc_genicam_camera: Grabbing thread stopped"); + RCLCPP_INFO(this->get_logger(), ("rc_genicam_camera: Grabbing thread stopped"); } } // namespace rcgccam -PLUGINLIB_EXPORT_CLASS(rcgccam::GenICamCameraNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(rcgccam::GenICamCameraNode, nodelet::Nodelet) From 731ab33821a87b789e2c863b9a9c05d7fb26ea0a Mon Sep 17 00:00:00 2001 From: MRo47 Date: Tue, 23 Aug 2022 23:37:48 +0100 Subject: [PATCH 08/32] updates to cam infor subs --- include/genicam_camera_node.h | 2 +- src/genicam_camera_nodelet.cc | 24 +++++++++++++----------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/include/genicam_camera_node.h b/include/genicam_camera_node.h index 8417848..fa62005 100644 --- a/include/genicam_camera_node.h +++ b/include/genicam_camera_node.h @@ -80,7 +80,7 @@ class GenICamCameraNode : public rclcpp::Node double timestamp_tolerance_; double sync_tolerance_; - rclcpp::Subscription::SharedPtr sub_sync_info_ptr_; + rclcpp::Subscription::SharedPtr sub_sync_info_ptr_; rclcpp::Service::SharedPtr get_param_service_ptr_; rclcpp::Service::SharedPtr set_param_service_ptr_; diff --git a/src/genicam_camera_nodelet.cc b/src/genicam_camera_nodelet.cc index 6783c40..4029326 100644 --- a/src/genicam_camera_nodelet.cc +++ b/src/genicam_camera_nodelet.cc @@ -70,10 +70,10 @@ explicit GenICamCameraNode::GenICamCameraNode(const std::string& node_name) this->declare_parameter("config_file"); this->declare_parameter("calib_file"); this->declare_parameter("calib_id"); - this->declare_parameter("host_timestamp"); - this->declare_parameter("timestamp_tolerance_"); + this->declare_parameter("host_timestamp", false); + this->declare_parameter("timestamp_tolerance_", 0.01); this->declare_parameter("sync_info"); - this->declare_parameter("sync_tolerance"); + this->declare_parameter("sync_tolerance", 0.019); this->declare_parameter("image_prefix"); this->declare_parameter("rotate"); } @@ -163,9 +163,8 @@ void GenICamCameraNode::onInit() // optional parameters for timestamp correction - bool host_timestamp = false; - pnh.param("host_timestamp", host_timestamp, host_timestamp); - pnh.param("timestamp_tolerance_", timestamp_tolerance_, 0.01); + auto host_timestamp = this->get_parameter("host_timestamp").as_bool(); + timestamp_tolerance_ = this->get_parameter("timestamp_tolerance_").as_double(); if (!host_timestamp) { @@ -174,13 +173,16 @@ void GenICamCameraNode::onInit() // optional parameters for timestamp synchronization - std::string sync_info; - pnh.param("sync_info", sync_info, sync_info); - pnh.param("sync_tolerance", sync_tolerance_, 0.019); + auto sync_info = this->get_parameter("sync_info").as_string(); + sync_tolerance_ = this->get_parameter("sync_tolerance").as_double(); if (sync_info.size() > 0) { - sub_sync_info_ = nh.subscribe(sync_info, 10, &GenICamCameraNode::syncInfo, this); + sub_sync_info_ptr_ = this->create_subscription( + sync_info, + rclcpp::SensorDataQoS(), + std::bind(&GenICamCameraNode::syncInfo, this, std::placeholders::_1) + ); image_list_.setSize(25); image_list_.setTolerance(static_cast(sync_tolerance_ * 1000000000.0)); @@ -400,7 +402,7 @@ void storeImage(const std::string &prefix, const sensor_msgs::ImagePtr &image) } -void GenICamCameraNode::syncInfo(sensor_msgs::CameraInfoPtr info) +void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::ConstSharedPtr info) { sensor_msgs::ImagePtr image; From b5acd65986b3a7368be5bdd351da432c21509d8d Mon Sep 17 00:00:00 2001 From: MRo47 Date: Wed, 24 Aug 2022 00:46:24 +0100 Subject: [PATCH 09/32] updated camera info publisher --- include/publishers/camera_info_publisher.h | 14 ++-- src/publishers/camera_info_publisher.cc | 92 +++++++++++----------- 2 files changed, 55 insertions(+), 51 deletions(-) diff --git a/include/publishers/camera_info_publisher.h b/include/publishers/camera_info_publisher.h index 0c1fe4f..8172a33 100644 --- a/include/publishers/camera_info_publisher.h +++ b/include/publishers/camera_info_publisher.h @@ -34,9 +34,9 @@ #ifndef RCGCCAM_CAMERAINFOPUBLISHER_H #define RCGCCAM_CAMERAINFOPUBLISHER_H -#include -#include -#include +#include +#include +#include #include @@ -57,18 +57,18 @@ class CameraInfoPublisher @param id Camera ID, i.e. < 0 for no ID, 0 for left and 1 for right camera. */ - void init(ros::NodeHandle& nh, const char* calib_file, int id); + void init(std::shared_ptr node, const char* calib_file, int id); bool used(); - void publish(const sensor_msgs::ImagePtr& image); + void publish(const sensor_msgs::msg::Image::ConstSharedPtr& image); private: CameraInfoPublisher(const CameraInfoPublisher&); // forbidden CameraInfoPublisher& operator=(const CameraInfoPublisher&); // forbidden - sensor_msgs::CameraInfo info_; - ros::Publisher pub_; + sensor_msgs::msg::CameraInfo info_; + rclcpp::Publisher::SharedPtr pub_ptr_; }; } // namespace rcgccam diff --git a/src/publishers/camera_info_publisher.cc b/src/publishers/camera_info_publisher.cc index 1f044f0..4451ae9 100644 --- a/src/publishers/camera_info_publisher.cc +++ b/src/publishers/camera_info_publisher.cc @@ -40,6 +40,7 @@ #include #include #include +#include namespace rcgccam { @@ -322,7 +323,7 @@ inline void transposeMatrix33(double M[3][3]) Store the given 3x3 matrix in a linear array. */ -inline void storeMatrix(boost::array& values, double M[3][3]) +inline void storeMatrix(std::array& values, double M[3][3]) { int j = 0; for (int k = 0; k < 3; k++) @@ -340,17 +341,20 @@ CameraInfoPublisher::CameraInfoPublisher() { } -void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int id) +void CameraInfoPublisher::init(std::shared_ptr node, const char* calib_file, int id) { // advertise topic - pub_ = nh.advertise("camera_info", 1); + pub_ptr_ = node->create_publisher( + "camera_info", + rclcpp::SensorDataQoS() + ); // check id if (id > 1) { - ROS_ERROR_STREAM("Invalid camera ID, only < 0, 0 and 1 allowed: " << id); + RCLCPP_ERROR_STREAM(node->get_logger(), "Invalid camera ID, only < 0, 0 and 1 allowed: " << id); return; } @@ -372,12 +376,12 @@ void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int double A[3][3]; if (!getMatrix33(data, createKey("A", id), A)) { - ROS_ERROR("Getting camera.A from calibration file failed"); - info_ = sensor_msgs::CameraInfo(); + RCLCPP_ERROR(node->get_logger(), "Getting camera.A from calibration file failed"); + info_ = sensor_msgs::msg::CameraInfo(); return; } - storeMatrix(info_.K, A); + storeMatrix(info_.k, A); // get lens distortion @@ -391,12 +395,12 @@ void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int if (e1 != 0 || e2 != 0 || e3 != 0 || e4 != 0) { info_.distortion_model = "equidistant"; - info_.D.resize(4); + info_.d.resize(4); - info_.D[0] = e1; - info_.D[1] = e2; - info_.D[2] = e3; - info_.D[3] = e4; + info_.d[0] = e1; + info_.d[1] = e2; + info_.d[2] = e3; + info_.d[3] = e4; } else { @@ -409,13 +413,13 @@ void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int getValue(data, createKey("p2", id), p2, "0"); info_.distortion_model = "plumb_bob"; - info_.D.resize(5); + info_.d.resize(5); - info_.D[0] = k1; - info_.D[1] = k2; - info_.D[2] = p1; - info_.D[3] = p2; - info_.D[4] = k3; + info_.d[0] = k1; + info_.d[1] = k2; + info_.d[2] = p1; + info_.d[3] = p2; + info_.d[4] = k3; } // determine focal length after rectification @@ -435,8 +439,8 @@ void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int } else { - ROS_ERROR("Getting camera.A0 and camera.A1 from calibration file failed"); - info_ = sensor_msgs::CameraInfo(); + RCLCPP_ERROR(node->get_logger(), "Getting camera.A0 and camera.A1 from calibration file failed"); + info_ = sensor_msgs::msg::CameraInfo(); return; } } @@ -501,7 +505,7 @@ void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int if (id == 0) { transposeMatrix33(Rrect); - storeMatrix(info_.R, Rrect); + storeMatrix(info_.r, Rrect); } else { @@ -509,36 +513,36 @@ void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int mulMatrix33Matrix33(Rrect_right, R, Rrect); transposeMatrix33(Rrect_right); - storeMatrix(info_.R, Rrect_right); + storeMatrix(info_.r, Rrect_right); } } else { // identity for mono camera - info_.R[1] = info_.R[2] = info_.R[3] = 0; - info_.R[0] = info_.R[4] = info_.R[8] = 1; - info_.R[5] = info_.R[6] = info_.R[7] = 0; + info_.r[1] = info_.r[2] = info_.r[3] = 0; + info_.r[0] = info_.r[4] = info_.r[8] = 1; + info_.r[5] = info_.r[6] = info_.r[7] = 0; } // define projection matrix after rectification - info_.P[0] = f; - info_.P[1] = 0; - info_.P[2] = info_.width/2.0; - info_.P[3] = 0; - info_.P[4] = 0; - info_.P[5] = f; - info_.P[6] = info_.height/2.0; - info_.P[7] = 0; - info_.P[8] = 0; - info_.P[9] = 0; - info_.P[10] = 1; - info_.P[11] = 0; + info_.p[0] = f; + info_.p[1] = 0; + info_.p[2] = info_.width/2.0; + info_.p[3] = 0; + info_.p[4] = 0; + info_.p[5] = f; + info_.p[6] = info_.height/2.0; + info_.p[7] = 0; + info_.p[8] = 0; + info_.p[9] = 0; + info_.p[10] = 1; + info_.p[11] = 0; if (id == 1) { - info_.P[3] = -f * t; + info_.p[3] = -f * t; } info_.binning_x = 1; @@ -546,29 +550,29 @@ void CameraInfoPublisher::init(ros::NodeHandle& nh, const char* calib_file, int } else { - ROS_ERROR_STREAM("gc_genicam_camera: Cannot load camera calibration: " << calib_file); + RCLCPP_ERROR_STREAM(node->get_logger(), "gc_genicam_camera: Cannot load camera calibration: " << calib_file); } } } bool CameraInfoPublisher::used() { - return pub_.getNumSubscribers() > 0; + return pub_ptr_->get_subscription_count() > 0; } -void CameraInfoPublisher::publish(const sensor_msgs::ImagePtr& image) +void CameraInfoPublisher::publish(const sensor_msgs::msg::Image::ConstSharedPtr& image) { - if (image && pub_.getNumSubscribers() > 0) + if (image && pub_ptr_->get_subscription_count() > 0) { info_.header = image->header; - if (info_.K[0] == 0) + if (info_.k[0] == 0) { info_.width = image->width; info_.height = image->height; } - pub_.publish(info_); + pub_ptr_->publish(info_); } } From 3726c14dd22dc9db21e7f57dad74795a54dd326e Mon Sep 17 00:00:00 2001 From: MRo47 Date: Wed, 24 Aug 2022 00:47:07 +0100 Subject: [PATCH 10/32] updated servers --- src/genicam_camera_nodelet.cc | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/genicam_camera_nodelet.cc b/src/genicam_camera_nodelet.cc index 4029326..8e9ae42 100644 --- a/src/genicam_camera_nodelet.cc +++ b/src/genicam_camera_nodelet.cc @@ -196,10 +196,15 @@ void GenICamCameraNode::onInit() } // setup service for getting and setting parameters - - get_param_service_ = pnh.advertiseService("get_genicam_parameter", &GenICamCameraNode::getGenICamParameter, this); - - set_param_service_ = pnh.advertiseService("set_genicam_parameter", &GenICamCameraNode::setGenICamParameter, this); + get_param_service_ptr_ = this->create_service( + "get_genicam_parameter", + std::bind(&GenICamCameraNode::getGenICamParameter, this, std::placeholders::_1) + ); + + set_param_service_ptr_ = this->create_service( + "set_genicam_parameter", + std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1) + ); // initialize publishers From 21760d1646488d258044144ebb3796b71cd385e3 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Wed, 24 Aug 2022 22:46:28 +0100 Subject: [PATCH 11/32] image publisher updated --- include/publishers/image_publisher.h | 8 ++++---- src/publishers/image_publisher.cc | 16 ++++++++-------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/include/publishers/image_publisher.h b/include/publishers/image_publisher.h index 0756bed..afac518 100644 --- a/include/publishers/image_publisher.h +++ b/include/publishers/image_publisher.h @@ -34,9 +34,9 @@ #ifndef RCGCCAM_IMAGEPUBLISHER_H #define RCGCCAM_IMAGEPUBLISHER_H -#include +#include #include -#include +#include #include @@ -58,7 +58,7 @@ class ImagePublisher bool used(); - void publish(const sensor_msgs::ImagePtr& image); + void publish(sensor_msgs::msg::Image::ConstSharedPtr image); private: ImagePublisher(const ImagePublisher&); // forbidden @@ -76,7 +76,7 @@ std::string rosPixelformat(int& bytes_per_pixel, uint64_t pixelformat); /** Converts a (supported) image in a GenICam buffer into a ROS image. */ -sensor_msgs::ImagePtr rosImageFromBuffer(const std::string& frame_id, const rcg::Buffer* buffer, +sensor_msgs::msg::Image::SharedPtr rosImageFromBuffer(const std::string& frame_id, const rcg::Buffer* buffer, uint32_t part, bool rotate); } // namespace rcgccam diff --git a/src/publishers/image_publisher.cc b/src/publishers/image_publisher.cc index 0539797..00b31a7 100644 --- a/src/publishers/image_publisher.cc +++ b/src/publishers/image_publisher.cc @@ -36,7 +36,7 @@ #include #include -#include +#include #ifdef __SSSE3__ #include @@ -58,7 +58,7 @@ bool ImagePublisher::used() return pub_.getNumSubscribers() > 0; } -void ImagePublisher::publish(const sensor_msgs::ImagePtr& image) +void ImagePublisher::publish(sensor_msgs::msg::Image::ConstSharedPtr image) { if (image && pub_.getNumSubscribers() > 0) { @@ -226,10 +226,10 @@ void copyInverse(uint8_t *tp, const uint8_t *sp, size_t nmemb, size_t m) } -sensor_msgs::ImagePtr rosImageFromBuffer(const std::string& frame_id, const rcg::Buffer* buffer, +sensor_msgs::msg::Image::SharedPtr rosImageFromBuffer(const std::string& frame_id, const rcg::Buffer* buffer, uint32_t part, bool rotate) { - sensor_msgs::ImagePtr im; + sensor_msgs::msg::Image::SharedPtr im; std::string pixelformat; int bytes_per_pixel; @@ -241,22 +241,22 @@ sensor_msgs::ImagePtr rosImageFromBuffer(const std::string& frame_id, const rcg: { rotate=false; - ROS_WARN_STREAM("Rotation is not supporte for image format: " << pixelformat); + RCLCPP_WARN_STREAM(rclcpp::get_logger("rc_genicam_camera::image_publisher::rosImageFromBuffer"), + "Rotation is not supporte for image format: " << pixelformat); } if (pixelformat.size() > 0) { // create image and initialize header - im = boost::make_shared(); + im = std::make_shared(); im->encoding = pixelformat; const uint64_t freq = 1000000000ul; uint64_t time = buffer->getTimestampNS(); - im->header.seq = 0; im->header.stamp.sec = time / freq; - im->header.stamp.nsec = time - freq * im->header.stamp.sec; + im->header.stamp.nanosec = time - freq * im->header.stamp.sec; im->header.frame_id = frame_id; // set image size From 4a0e3373ee418bedffa8321a59954b38c1e2f5e3 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Wed, 24 Aug 2022 23:12:21 +0100 Subject: [PATCH 12/32] cam info pub updated with shared ptr --- include/publishers/camera_info_publisher.h | 2 +- src/genicam_camera_nodelet.cc | 14 +++++--------- src/publishers/camera_info_publisher.cc | 2 +- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/include/publishers/camera_info_publisher.h b/include/publishers/camera_info_publisher.h index 8172a33..17ee1e1 100644 --- a/include/publishers/camera_info_publisher.h +++ b/include/publishers/camera_info_publisher.h @@ -57,7 +57,7 @@ class CameraInfoPublisher @param id Camera ID, i.e. < 0 for no ID, 0 for left and 1 for right camera. */ - void init(std::shared_ptr node, const char* calib_file, int id); + void init(rclcpp::Node::SharedPtr node, const char* calib_file, int id); bool used(); diff --git a/src/genicam_camera_nodelet.cc b/src/genicam_camera_nodelet.cc index 8e9ae42..72ddc90 100644 --- a/src/genicam_camera_nodelet.cc +++ b/src/genicam_camera_nodelet.cc @@ -207,22 +207,18 @@ void GenICamCameraNode::onInit() ); // initialize publishers - - caminfo_pub_.init(nh, calib.c_str(), calib_id); - - image_transport::ImageTransport it(nh); + caminfo_pub_.init(this->shared_from_this(), calib.c_str(), calib_id); + + image_transport::ImageTransport it(this->shared_from_this()); image_pub_.init(it); // get optional prefix for storing all grabbed images - - pnh.param("image_prefix", image_prefix_, image_prefix_); + image_prefix_ = this->get_parameter("image_prefix").as_string(); // rotating images by 180 degrees? - - pnh.param("rotate", rotate_, rotate_); + rotate_ = this->get_parameter("rotate").as_bool(); // start grabbing threads - running_ = true; grab_thread_ = std::thread(&GenICamCameraNode::grab, this, device, access_id, config); } diff --git a/src/publishers/camera_info_publisher.cc b/src/publishers/camera_info_publisher.cc index 4451ae9..3b6fb1c 100644 --- a/src/publishers/camera_info_publisher.cc +++ b/src/publishers/camera_info_publisher.cc @@ -341,7 +341,7 @@ CameraInfoPublisher::CameraInfoPublisher() { } -void CameraInfoPublisher::init(std::shared_ptr node, const char* calib_file, int id) +void CameraInfoPublisher::init(rclcpp::Node::SharedPtr node, const char* calib_file, int id) { // advertise topic From 21ce43d093d47823fbd64a3200677fd1119bc231 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Thu, 25 Aug 2022 00:45:51 +0100 Subject: [PATCH 13/32] non const messages --- include/camerainfolist.h | 6 +++--- src/camerainfolist.cc | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/camerainfolist.h b/include/camerainfolist.h index 60194aa..feb748e 100644 --- a/include/camerainfolist.h +++ b/include/camerainfolist.h @@ -73,7 +73,7 @@ class CameraInfoList @return Dropped camera info message, null pointer if nothing is dropped. */ - sensor_msgs::msg::CameraInfo::ConstSharedPtr add(sensor_msgs::msg::CameraInfo::ConstSharedPtr info); + sensor_msgs::msg::CameraInfo::SharedPtr add(sensor_msgs::msg::CameraInfo::SharedPtr info); /** Remove all camera infos that have a timestamp that is older or equal than @@ -93,12 +93,12 @@ class CameraInfoList @param tolerance Maximum tolarance added or subtracted to the timestamp. @return Pointer to camera info or 0. */ - sensor_msgs::msg::CameraInfo::ConstSharedPtr find(const rclcpp::Time& timestamp) const; + sensor_msgs::msg::CameraInfo::SharedPtr find(const rclcpp::Time& timestamp) const; private: size_t maxsize_; uint64_t tolerance_; - std::vector list_; + std::vector list_; }; } // namespace rcgccam diff --git a/src/camerainfolist.cc b/src/camerainfolist.cc index b8269c9..824fa53 100644 --- a/src/camerainfolist.cc +++ b/src/camerainfolist.cc @@ -54,11 +54,11 @@ void CameraInfoList::setTolerance(uint64_t tolerance) tolerance_ = tolerance; } -sensor_msgs::msg::CameraInfo::ConstSharedPtr CameraInfoList::add(sensor_msgs::msg::CameraInfo::ConstSharedPtr info) +sensor_msgs::msg::CameraInfo::SharedPtr CameraInfoList::add(sensor_msgs::msg::CameraInfo::SharedPtr info) { list_.push_back(info); - sensor_msgs::msg::CameraInfo::ConstSharedPtr ret; + sensor_msgs::msg::CameraInfo::SharedPtr ret; if (list_.size() > maxsize_) { @@ -90,7 +90,7 @@ int CameraInfoList::removeOld(const rclcpp::Time& timestamp) return n; } -sensor_msgs::msg::CameraInfo::ConstSharedPtr CameraInfoList::find(const rclcpp::Time& timestamp) const +sensor_msgs::msg::CameraInfo::SharedPtr CameraInfoList::find(const rclcpp::Time& timestamp) const { for (size_t i = 0; i < list_.size(); i++) { @@ -103,7 +103,7 @@ sensor_msgs::msg::CameraInfo::ConstSharedPtr CameraInfoList::find(const rclcpp:: } } - return sensor_msgs::msg::CameraInfo::ConstSharedPtr(); + return sensor_msgs::msg::CameraInfo::SharedPtr(); } } // namespace rcgccam From ca6712ca839f5e67357d607476d3352c91f3229e Mon Sep 17 00:00:00 2001 From: MRo47 Date: Thu, 25 Aug 2022 00:46:12 +0100 Subject: [PATCH 14/32] non const messages --- include/imagelist.h | 6 +++--- src/imagelist.cc | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/imagelist.h b/include/imagelist.h index 095fd82..3c736af 100644 --- a/include/imagelist.h +++ b/include/imagelist.h @@ -71,7 +71,7 @@ class ImageList @param image Image to be added. @return Dropped image, null pointer if no image is dropped. */ - sensor_msgs::msg::Image::ConstSharedPtr add(sensor_msgs::msg::Image::ConstSharedPtr image); + sensor_msgs::msg::Image::SharedPtr add(sensor_msgs::msg::Image::SharedPtr image); /** Remove all images that have a timestamp that is older or equal than the @@ -91,12 +91,12 @@ class ImageList @param tolerance Maximum tolarance added or subtracted to the timestamp. @return Pointer to image or 0. */ - sensor_msgs::msg::Image::ConstSharedPtr find(const rclcpp::Time& timestamp) const; + sensor_msgs::msg::Image::SharedPtr find(const rclcpp::Time& timestamp) const; private: size_t maxsize_; uint64_t tolerance_; - std::vector list_; + std::vector list_; }; } // namespace rcgccam diff --git a/src/imagelist.cc b/src/imagelist.cc index b2bf901..945bc6f 100644 --- a/src/imagelist.cc +++ b/src/imagelist.cc @@ -54,11 +54,11 @@ void ImageList::setTolerance(uint64_t tolerance) tolerance_ = tolerance; } -sensor_msgs::msg::Image::ConstSharedPtr ImageList::add(sensor_msgs::msg::Image::ConstSharedPtr image) +sensor_msgs::msg::Image::SharedPtr ImageList::add(sensor_msgs::msg::Image::SharedPtr image) { list_.push_back(image); - sensor_msgs::msg::Image::ConstSharedPtr ret; + sensor_msgs::msg::Image::SharedPtr ret; if (list_.size() > maxsize_) { @@ -90,7 +90,7 @@ int ImageList::removeOld(const rclcpp::Time& timestamp) return n; } -sensor_msgs::msg::Image::ConstSharedPtr ImageList::find(const rclcpp::Time& timestamp) const +sensor_msgs::msg::Image::SharedPtr ImageList::find(const rclcpp::Time& timestamp) const { for (size_t i = 0; i < list_.size(); i++) { @@ -103,7 +103,7 @@ sensor_msgs::msg::Image::ConstSharedPtr ImageList::find(const rclcpp::Time& time } } - return sensor_msgs::msg::Image::ConstSharedPtr(); + return sensor_msgs::msg::Image::SharedPtr(); } } // namespace rcgccam From 69a1280222e1f11721370a06681170bdfec41e4e Mon Sep 17 00:00:00 2001 From: MRo47 Date: Thu, 25 Aug 2022 00:47:03 +0100 Subject: [PATCH 15/32] time of type builtin interfaces --- include/timestamp_corrector.h | 2 +- src/timestamp_corrector.cc | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/timestamp_corrector.h b/include/timestamp_corrector.h index a824e19..c10680f 100644 --- a/include/timestamp_corrector.h +++ b/include/timestamp_corrector.h @@ -90,7 +90,7 @@ class TimestampCorrector determineOffset() has never been called or if it delivered an error on the last call. */ - int64_t correct(rclcpp::Time& time); + int64_t correct(builtin_interfaces::msg::Time& time); private: int64_t tolerance_; diff --git a/src/timestamp_corrector.cc b/src/timestamp_corrector.cc index acf4d40..da720bf 100644 --- a/src/timestamp_corrector.cc +++ b/src/timestamp_corrector.cc @@ -132,11 +132,11 @@ bool TimestampCorrector::determineOffset(const std::shared_ptr= 0 && accuracy_ >= 0) { - int64_t t = static_cast(time.nanoseconds()); + int64_t t = static_cast(time.nanosec); time = rclcpp::Time(static_cast(t + offset_)); return accuracy_; From 67251a74e1689bde2529bcbd9e2d017165e41f17 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Thu, 25 Aug 2022 00:47:47 +0100 Subject: [PATCH 16/32] updated complete genicam camera node --- include/genicam_camera_node.h | 2 +- src/genicam_camera_nodelet.cc | 45 +++++++++++++++++++---------------- 2 files changed, 26 insertions(+), 21 deletions(-) diff --git a/include/genicam_camera_node.h b/include/genicam_camera_node.h index fa62005..b92fa13 100644 --- a/include/genicam_camera_node.h +++ b/include/genicam_camera_node.h @@ -71,7 +71,7 @@ class GenICamCameraNode : public rclcpp::Node bool setGenICamParameter(rc_genicam_camera::srv::SetGenICamParameter::Request& req, rc_genicam_camera::srv::SetGenICamParameter::Response& resp); - void syncInfo(sensor_msgs::msg::CameraInfo::ConstSharedPtr info); + void syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info); private: diff --git a/src/genicam_camera_nodelet.cc b/src/genicam_camera_nodelet.cc index 72ddc90..896183d 100644 --- a/src/genicam_camera_nodelet.cc +++ b/src/genicam_camera_nodelet.cc @@ -47,6 +47,9 @@ #include #include #include +#include +#include +#include #include #include @@ -241,7 +244,8 @@ std::string loadConfig(const std::string& filename) } else { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot load config: " << filename); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::loadConfig"), + "rc_genicam_camera: Cannot load config: " << filename); } } @@ -302,8 +306,8 @@ void applyParameters(const std::shared_ptr& nodemap, const } // namespace -bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::GetGenICamParameter::Request& req, - rc_genicam_camera::GetGenICamParameter::Response& resp) +bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::srv::GetGenICamParameter::Request& req, + rc_genicam_camera::srv::GetGenICamParameter::Response& resp) { std::lock_guard lock(device_mtx_); @@ -327,8 +331,8 @@ bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::GetGenICamParamet return true; } -bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera::SetGenICamParameter::Request& req, - rc_genicam_camera::SetGenICamParameter::Response& resp) +bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera::srv::SetGenICamParameter::Request& req, + rc_genicam_camera::srv::SetGenICamParameter::Response& resp) { std::lock_guard lock(device_mtx_); @@ -356,14 +360,14 @@ bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera::SetGenICamParamet namespace { -void storeImage(const std::string &prefix, const sensor_msgs::ImagePtr &image) +void storeImage(const std::string &prefix, const sensor_msgs::msg::Image::SharedPtr image) { // prepare file name std::ostringstream name; uint64_t t_sec = image->header.stamp.sec; - uint64_t t_nsec = image->header.stamp.nsec; + uint64_t t_nsec = image->header.stamp.nanosec; name << prefix << "_" << t_sec << "." << std::setfill('0') << std::setw(9) << t_nsec << ".pgm"; @@ -388,24 +392,26 @@ void storeImage(const std::string &prefix, const sensor_msgs::ImagePtr &image) if (n < width*height) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot write to file " << name.str() << - " (" << n << " < " << width*height << ")"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), + "Cannot write to file " << name.str() << " (" + << n << " < " << width*height << ")"); } fclose(out); } else { - RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot create file " << name.str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), + "Cannot create file " << name.str()); } } } } -void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::ConstSharedPtr info) +void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info) { - sensor_msgs::ImagePtr image; + sensor_msgs::msg::Image::SharedPtr image; { std::lock_guard lock(sync_mtx_); @@ -526,7 +532,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std stream[0]->open(); stream[0]->startStreaming(); - ROS_INFO_STREAM("rc_genicam_camera: Start streaming"); + RCLCPP_INFO_STREAM(this->get_logger(), "rc_genicam_camera: Start streaming"); // grabbing thread @@ -545,12 +551,11 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std { // convert image to ROS - sensor_msgs::ImagePtr image = rosImageFromBuffer(frame_id_, buffer, part, rotate_); + sensor_msgs::msg::Image::SharedPtr image = rosImageFromBuffer(frame_id_, buffer, part, rotate_); if (image) { // correct timestamp - ts_host.correct(image->header.stamp); // optionally take timestamp of approximately synchronized @@ -562,7 +567,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std // find camera info that corresponds to the image - sensor_msgs::CameraInfoPtr info = info_list_.find(image->header.stamp); + sensor_msgs::msg::CameraInfo::SharedPtr info = info_list_.find(image->header.stamp); if (info != 0) { @@ -589,7 +594,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std if (image) { - RCLCPP_WARN_THROTTLE(this->get_logger(), 10, "rc_genicam_camera: Input queue full, dropping image"); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "rc_genicam_camera: Input queue full, dropping image"); } image.reset(); @@ -642,7 +647,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); - sleep(1); + std::this_thread::sleep_for(std::chrono::seconds(1)); } // close device @@ -668,9 +673,9 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std } running_ = false; - RCLCPP_INFO(this->get_logger(), ("rc_genicam_camera: Grabbing thread stopped"); + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread stopped"); } } // namespace rcgccam -PLUGINLIB_EXPORT_CLASS(rcgccam::GenICamCameraNode, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(rcgccam::GenICamCameraNode, rclcpp::Node) From ee96f226ea0e03027cb2b04b998ce67349be2704 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Thu, 25 Aug 2022 23:55:34 +0100 Subject: [PATCH 17/32] renamed main nodelet to node --- src/genicam_camera_node.cc | 652 +++++++++++++++++++++++++++++- src/genicam_camera_node_main.cc | 49 +++ src/genicam_camera_nodelet.cc | 681 -------------------------------- 3 files changed, 691 insertions(+), 691 deletions(-) create mode 100644 src/genicam_camera_node_main.cc delete mode 100644 src/genicam_camera_nodelet.cc diff --git a/src/genicam_camera_node.cc b/src/genicam_camera_node.cc index 05f821a..896183d 100644 --- a/src/genicam_camera_node.cc +++ b/src/genicam_camera_node.cc @@ -31,19 +31,651 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "genicam_camera_node.h" +#include "timestamp_corrector.h" -int main(int argc, char** argv) +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rcgccam +{ +// TODO check this +#define ROS_HAS_STEADYTIME (ROS_VERSION_MINIMUM(1, 13, 1) || ((ROS_VERSION_MINOR == 12) && ROS_VERSION_PATCH >= 8)) + +explicit GenICamCameraNode::GenICamCameraNode(const std::string& node_name) +: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) +{ + timestamp_tolerance_ = 0; + sync_tolerance_ = 0; + rotate_ = false; + running_ = false; + + this->declare_parameter("frame_id"); + this->declare_parameter("device"); + this->declare_parameter("gev_access"); + this->declare_parameter("config_file"); + this->declare_parameter("calib_file"); + this->declare_parameter("calib_id"); + this->declare_parameter("host_timestamp", false); + this->declare_parameter("timestamp_tolerance_", 0.01); + this->declare_parameter("sync_info"); + this->declare_parameter("sync_tolerance", 0.019); + this->declare_parameter("image_prefix"); + this->declare_parameter("rotate"); +} + +GenICamCameraNode::~GenICamCameraNode() +{ + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Shutting down"); + + // signal running_ threads and wait until they finish + + running_ = false; + if (grab_thread_.joinable()) + { + grab_thread_.join(); + } + + rcg::System::clearSystems(); +} + +void GenICamCameraNode::onInit() +{ + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Initialization"); + + // get parameter configuration + + std::string device = ""; + std::string access = "control"; + std::string config = ""; + std::string calib = ""; + int calib_id=-1; + + frame_id_ = this->get_parameter("frame_id").as_string(); + + if (frame_id_.size() == 0) + { + std::string ns = this->get_namespace(); + + if (ns.size() > 0 && ns[0] == '/') + { + ns = ns.substr(1); + } + + if (ns.size() > 0) + { + frame_id_ = ns + "_camera"; + } + else + { + frame_id_ = "camera"; + } + + int cid=-1; + cid = this->get_parameter("calib_id").as_int(); + + if (cid >= 0) + { + frame_id_ = frame_id_ + std::to_string(cid); + } + } + + device = this->get_parameter("device").as_string(); + access = this->get_parameter("gev_access").as_string(); + config = this->get_parameter("config_file").as_string(); + calib = this->get_parameter("calib_file").as_string(); + calib_id = this->get_parameter("calib_id").as_int(); + + if (device.size() == 0) + { + RCLCPP_FATAL(this->get_logger(), "The GenICam device ID must be given in the private parameter 'device'!"); + return; + } + + rcg::Device::ACCESS access_id; + if (access == "exclusive") + { + access_id = rcg::Device::EXCLUSIVE; + } + else if (access == "control") + { + access_id = rcg::Device::CONTROL; + } + else + { + RCLCPP_FATAL_STREAM(this->get_logger(), "rc_visard_driver: Access must be 'control' or 'exclusive': " << access); + return; + } + + // optional parameters for timestamp correction + + auto host_timestamp = this->get_parameter("host_timestamp").as_bool(); + timestamp_tolerance_ = this->get_parameter("timestamp_tolerance_").as_double(); + + if (!host_timestamp) + { + timestamp_tolerance_ = -1; + } + + // optional parameters for timestamp synchronization + + auto sync_info = this->get_parameter("sync_info").as_string(); + sync_tolerance_ = this->get_parameter("sync_tolerance").as_double(); + + if (sync_info.size() > 0) + { + sub_sync_info_ptr_ = this->create_subscription( + sync_info, + rclcpp::SensorDataQoS(), + std::bind(&GenICamCameraNode::syncInfo, this, std::placeholders::_1) + ); + + image_list_.setSize(25); + image_list_.setTolerance(static_cast(sync_tolerance_ * 1000000000.0)); + + info_list_.setSize(25); + info_list_.setTolerance(static_cast(sync_tolerance_ * 1000000000.0)); + } + else + { + sync_tolerance_ = -1; + } + + // setup service for getting and setting parameters + get_param_service_ptr_ = this->create_service( + "get_genicam_parameter", + std::bind(&GenICamCameraNode::getGenICamParameter, this, std::placeholders::_1) + ); + + set_param_service_ptr_ = this->create_service( + "set_genicam_parameter", + std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1) + ); + + // initialize publishers + caminfo_pub_.init(this->shared_from_this(), calib.c_str(), calib_id); + + image_transport::ImageTransport it(this->shared_from_this()); + image_pub_.init(it); + + // get optional prefix for storing all grabbed images + image_prefix_ = this->get_parameter("image_prefix").as_string(); + + // rotating images by 180 degrees? + rotate_ = this->get_parameter("rotate").as_bool(); + + // start grabbing threads + running_ = true; + grab_thread_ = std::thread(&GenICamCameraNode::grab, this, device, access_id, config); +} + +namespace +{ +// read file as string + +std::string loadConfig(const std::string& filename) +{ + if (filename.size() > 0) + { + std::ifstream in(filename); + std::stringstream buffer; + + if (in) + { + buffer << in.rdbuf(); + return buffer.str(); + } + else + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::loadConfig"), + "rc_genicam_camera: Cannot load config: " << filename); + } + } + + return std::string(); +} + +// Expect one or more GenICam parameter name and values in the format +// [=], which must not contain a white space, separated by white +// spaces and applies them to the given nodemap. +// +// An expection is thrown in case of an error. Execution stops on the first +// error. + +void applyParameters(const std::shared_ptr& nodemap, const std::string& parameters) +{ + size_t i = 0; + + while (i < parameters.size()) + { + // eat all white spaces + + while (i < parameters.size() && std::isspace(parameters[i])) + i++; + + // skip comments which start with # and end by \n + + if (i < parameters.size() && parameters[i] == '#') + { + while (i < parameters.size() && parameters[i] != '\n') + i++; + } + else + { + size_t k = i; + while (k < parameters.size() && !std::isspace(parameters[k])) + k++; + + size_t j = parameters.find('=', i); + + if (j <= k) + { + std::string name = parameters.substr(i, j - i); + std::string value = parameters.substr(j + 1, k - j - 1); + + rcg::setString(nodemap, name.c_str(), value.c_str(), true); + } + else if (i < k) + { + std::string name = parameters.substr(i, k - i); + + rcg::callCommand(nodemap, name.c_str(), true); + } + + i = k; + } + } +} + +} // namespace + +bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::srv::GetGenICamParameter::Request& req, + rc_genicam_camera::srv::GetGenICamParameter::Response& resp) +{ + std::lock_guard lock(device_mtx_); + + if (rcgnodemap_) + { + try + { + resp.value = rcg::getString(rcgnodemap_, req.name.c_str(), true); + resp.return_code.value = resp.return_code.SUCCESS; + resp.return_code.message = "ok"; + } + catch (const std::exception& ex) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot get parameter: " << ex.what()); + + resp.return_code.value = resp.return_code.INVALID_ARGUMENT; + resp.return_code.message = ex.what(); + } + } + + return true; +} + +bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera::srv::SetGenICamParameter::Request& req, + rc_genicam_camera::srv::SetGenICamParameter::Response& resp) +{ + std::lock_guard lock(device_mtx_); + + if (rcgnodemap_) + { + try + { + applyParameters(rcgnodemap_, req.parameters); + + resp.return_code.value = resp.return_code.SUCCESS; + resp.return_code.message = "ok"; + } + catch (const std::exception& ex) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot set parameters: " << ex.what()); + + resp.return_code.value = resp.return_code.INVALID_ARGUMENT; + resp.return_code.message = ex.what(); + } + } + + return true; +} + +namespace +{ + +void storeImage(const std::string &prefix, const sensor_msgs::msg::Image::SharedPtr image) { - ros::init(argc, argv, "rc_genicam_camera"); + // prepare file name - nodelet::Loader manager(false); - nodelet::M_string remappings(ros::names::getRemappings()); - nodelet::V_string my_argv(argv + 1, argv + argc); + std::ostringstream name; - manager.load(ros::this_node::getName(), "rc_genicam_camera", remappings, my_argv); + uint64_t t_sec = image->header.stamp.sec; + uint64_t t_nsec = image->header.stamp.nanosec; - ros::spin(); - return 0; + name << prefix << "_" << t_sec << "." << std::setfill('0') << std::setw(9) << t_nsec << ".pgm"; + + // store 8 bit images + + if (image->encoding == sensor_msgs::image_encodings::MONO8 || + image->encoding == sensor_msgs::image_encodings::BAYER_BGGR8 || + image->encoding == sensor_msgs::image_encodings::BAYER_GBRG8 || + image->encoding == sensor_msgs::image_encodings::BAYER_GRBG8 || + image->encoding == sensor_msgs::image_encodings::BAYER_RGGB8) + { + size_t width = image->width; + size_t height = image->height; + uint8_t* p = reinterpret_cast(&image->data[0]); + + FILE *out = fopen(name.str().c_str(), "w"); + + if (out) + { + fprintf(out, "P5\n%lu %lu\n255\n", width, height); + size_t n = fwrite(p, 1, width*height, out); + + if (n < width*height) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), + "Cannot write to file " << name.str() << " (" + << n << " < " << width*height << ")"); + } + + fclose(out); + } + else + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), + "Cannot create file " << name.str()); + } + } +} + +} + +void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info) +{ + sensor_msgs::msg::Image::SharedPtr image; + + { + std::lock_guard lock(sync_mtx_); + + // find image that corresponds to camera info + + image = image_list_.find(info->header.stamp); + + if (image != 0) + { + // remove all older images and infos + + int n = image_list_.removeOld(image->header.stamp) - 1; + info_list_.removeOld(info->header.stamp); + + if (n > 0) + { + RCLCPP_WARN_STREAM(this->get_logger(),"rc_genicam_camera: Dropped unused images: " << n); + } + + // correct time stamp of image + + image->header.stamp = info->header.stamp; + } + else + { + // store info + + info_list_.add(info); + } + } + + // publish images + + if (image) + { + caminfo_pub_.publish(image); + image_pub_.publish(image); + + // store images + + if (image_prefix_.size() > 0) + { + storeImage(image_prefix_, image); + } + } } + +void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std::string config) +{ + try + { + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread started"); + + // load initial configuration for camera into string + + std::string init_params = loadConfig(config); + + // initialize optional timestamp correction + + TimestampCorrector ts_host; + ts_host.setMaximumTolerance(static_cast(timestamp_tolerance_ * 1000000000)); + ts_host.setInterval(1000000000ll); + + // loop until nodelet is killed + + while (running_) + { + // report standard exceptions and try again + + try + { + { + std::lock_guard lock(device_mtx_); + + // open device and get nodemap + + rcgdev_ = rcg::getDevice(device.c_str()); + + if (!rcgdev_) + { + throw std::invalid_argument("Cannot find device"); + } + + rcgdev_->open(access); + rcgnodemap_ = rcgdev_->getRemoteNodeMap(); + + // initialize camera + + try + { + applyParameters(rcgnodemap_, init_params); + } + catch (const std::exception& ex) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Error during initial camera configuration: " << ex.what()); + } + } + + // initialize timestamp correction + + if (!ts_host.determineOffset(rcgnodemap_)) + { + RCLCPP_ERROR_STREAM(this->get_logger(), + "rc_genicam_camera: Cannot determine offset between host and camera clock with maximum tolerance of " + << timestamp_tolerance_ << " s"); + } + + // start streaming + + std::vector > stream = rcgdev_->getStreams(); + + if (stream.size() == 0) + { + throw std::invalid_argument("Device does not offer streams"); + } + + stream[0]->open(); + stream[0]->startStreaming(); + + RCLCPP_INFO_STREAM(this->get_logger(), "rc_genicam_camera: Start streaming"); + + // grabbing thread + + while (running_) + { + const rcg::Buffer* buffer = stream[0]->grab(50); + + if (buffer != 0) + { + if (!buffer->getIsIncomplete()) + { + uint32_t npart = buffer->getNumberOfParts(); + for (uint32_t part = 0; part < npart; part++) + { + if (buffer->getImagePresent(part)) + { + // convert image to ROS + + sensor_msgs::msg::Image::SharedPtr image = rosImageFromBuffer(frame_id_, buffer, part, rotate_); + + if (image) + { + // correct timestamp + ts_host.correct(image->header.stamp); + + // optionally take timestamp of approximately synchronized + // camera info + + if (sync_tolerance_ > 0) + { + std::lock_guard lock(sync_mtx_); + + // find camera info that corresponds to the image + + sensor_msgs::msg::CameraInfo::SharedPtr info = info_list_.find(image->header.stamp); + + if (info != 0) + { + // remove all older images and infos + + int n = image_list_.removeOld(image->header.stamp); + info_list_.removeOld(info->header.stamp); + + if (n > 0) + { + RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Dropped images: " << n); + } + + // correct time stamp of image + + image->header.stamp = info->header.stamp; + } + else + { + // store image in internal list for later + // synchronization to camera info + + image = image_list_.add(image); + + if (image) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "rc_genicam_camera: Input queue full, dropping image"); + } + + image.reset(); + } + } + + // publish images + + if (image) + { + caminfo_pub_.publish(image); + image_pub_.publish(image); + + // store images + + if (image_prefix_.size() > 0) + { + storeImage(image_prefix_, image); + } + } + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Unsupported pixel format"); + } + } + } + + // re-determine offset of host and camera clock + + if (!ts_host.determineOffset(rcgnodemap_)) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot determine offset between host and camera clock with " + "maximum tolerance of " + << timestamp_tolerance_ << " s"); + } + } + else + { + RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Incomplete buffer received"); + } + } + } + + // stop streaming + + stream[0]->stopStreaming(); + stream[0]->close(); + } + catch (const std::exception& ex) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // close device + + { + std::lock_guard lock(device_mtx_); + + if (rcgdev_) + rcgdev_->close(); + + rcgdev_.reset(); + rcgnodemap_.reset(); + } + } + } + catch (const std::exception& ex) + { + RCLCPP_FATAL_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); + } + catch (...) + { + RCLCPP_FATAL_STREAM(this->get_logger(),"rc_genicam_camera: Unknown exception"); + } + + running_ = false; + RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread stopped"); +} + +} // namespace rcgccam + +PLUGINLIB_EXPORT_CLASS(rcgccam::GenICamCameraNode, rclcpp::Node) diff --git a/src/genicam_camera_node_main.cc b/src/genicam_camera_node_main.cc new file mode 100644 index 0000000..05f821a --- /dev/null +++ b/src/genicam_camera_node_main.cc @@ -0,0 +1,49 @@ +/* + * Copyright (c) 2019 Roboception GmbH + * All rights reserved + * + * Author: Heiko Hirschmueller + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "rc_genicam_camera"); + + nodelet::Loader manager(false); + nodelet::M_string remappings(ros::names::getRemappings()); + nodelet::V_string my_argv(argv + 1, argv + argc); + + manager.load(ros::this_node::getName(), "rc_genicam_camera", remappings, my_argv); + + ros::spin(); + return 0; +} diff --git a/src/genicam_camera_nodelet.cc b/src/genicam_camera_nodelet.cc deleted file mode 100644 index 896183d..0000000 --- a/src/genicam_camera_nodelet.cc +++ /dev/null @@ -1,681 +0,0 @@ -/* - * Copyright (c) 2019 Roboception GmbH - * All rights reserved - * - * Author: Heiko Hirschmueller - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "genicam_camera_node.h" -#include "timestamp_corrector.h" - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace rcgccam -{ -// TODO check this -#define ROS_HAS_STEADYTIME (ROS_VERSION_MINIMUM(1, 13, 1) || ((ROS_VERSION_MINOR == 12) && ROS_VERSION_PATCH >= 8)) - -explicit GenICamCameraNode::GenICamCameraNode(const std::string& node_name) -: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) -{ - timestamp_tolerance_ = 0; - sync_tolerance_ = 0; - rotate_ = false; - running_ = false; - - this->declare_parameter("frame_id"); - this->declare_parameter("device"); - this->declare_parameter("gev_access"); - this->declare_parameter("config_file"); - this->declare_parameter("calib_file"); - this->declare_parameter("calib_id"); - this->declare_parameter("host_timestamp", false); - this->declare_parameter("timestamp_tolerance_", 0.01); - this->declare_parameter("sync_info"); - this->declare_parameter("sync_tolerance", 0.019); - this->declare_parameter("image_prefix"); - this->declare_parameter("rotate"); -} - -GenICamCameraNode::~GenICamCameraNode() -{ - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Shutting down"); - - // signal running_ threads and wait until they finish - - running_ = false; - if (grab_thread_.joinable()) - { - grab_thread_.join(); - } - - rcg::System::clearSystems(); -} - -void GenICamCameraNode::onInit() -{ - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Initialization"); - - // get parameter configuration - - std::string device = ""; - std::string access = "control"; - std::string config = ""; - std::string calib = ""; - int calib_id=-1; - - frame_id_ = this->get_parameter("frame_id").as_string(); - - if (frame_id_.size() == 0) - { - std::string ns = this->get_namespace(); - - if (ns.size() > 0 && ns[0] == '/') - { - ns = ns.substr(1); - } - - if (ns.size() > 0) - { - frame_id_ = ns + "_camera"; - } - else - { - frame_id_ = "camera"; - } - - int cid=-1; - cid = this->get_parameter("calib_id").as_int(); - - if (cid >= 0) - { - frame_id_ = frame_id_ + std::to_string(cid); - } - } - - device = this->get_parameter("device").as_string(); - access = this->get_parameter("gev_access").as_string(); - config = this->get_parameter("config_file").as_string(); - calib = this->get_parameter("calib_file").as_string(); - calib_id = this->get_parameter("calib_id").as_int(); - - if (device.size() == 0) - { - RCLCPP_FATAL(this->get_logger(), "The GenICam device ID must be given in the private parameter 'device'!"); - return; - } - - rcg::Device::ACCESS access_id; - if (access == "exclusive") - { - access_id = rcg::Device::EXCLUSIVE; - } - else if (access == "control") - { - access_id = rcg::Device::CONTROL; - } - else - { - RCLCPP_FATAL_STREAM(this->get_logger(), "rc_visard_driver: Access must be 'control' or 'exclusive': " << access); - return; - } - - // optional parameters for timestamp correction - - auto host_timestamp = this->get_parameter("host_timestamp").as_bool(); - timestamp_tolerance_ = this->get_parameter("timestamp_tolerance_").as_double(); - - if (!host_timestamp) - { - timestamp_tolerance_ = -1; - } - - // optional parameters for timestamp synchronization - - auto sync_info = this->get_parameter("sync_info").as_string(); - sync_tolerance_ = this->get_parameter("sync_tolerance").as_double(); - - if (sync_info.size() > 0) - { - sub_sync_info_ptr_ = this->create_subscription( - sync_info, - rclcpp::SensorDataQoS(), - std::bind(&GenICamCameraNode::syncInfo, this, std::placeholders::_1) - ); - - image_list_.setSize(25); - image_list_.setTolerance(static_cast(sync_tolerance_ * 1000000000.0)); - - info_list_.setSize(25); - info_list_.setTolerance(static_cast(sync_tolerance_ * 1000000000.0)); - } - else - { - sync_tolerance_ = -1; - } - - // setup service for getting and setting parameters - get_param_service_ptr_ = this->create_service( - "get_genicam_parameter", - std::bind(&GenICamCameraNode::getGenICamParameter, this, std::placeholders::_1) - ); - - set_param_service_ptr_ = this->create_service( - "set_genicam_parameter", - std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1) - ); - - // initialize publishers - caminfo_pub_.init(this->shared_from_this(), calib.c_str(), calib_id); - - image_transport::ImageTransport it(this->shared_from_this()); - image_pub_.init(it); - - // get optional prefix for storing all grabbed images - image_prefix_ = this->get_parameter("image_prefix").as_string(); - - // rotating images by 180 degrees? - rotate_ = this->get_parameter("rotate").as_bool(); - - // start grabbing threads - running_ = true; - grab_thread_ = std::thread(&GenICamCameraNode::grab, this, device, access_id, config); -} - -namespace -{ -// read file as string - -std::string loadConfig(const std::string& filename) -{ - if (filename.size() > 0) - { - std::ifstream in(filename); - std::stringstream buffer; - - if (in) - { - buffer << in.rdbuf(); - return buffer.str(); - } - else - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::loadConfig"), - "rc_genicam_camera: Cannot load config: " << filename); - } - } - - return std::string(); -} - -// Expect one or more GenICam parameter name and values in the format -// [=], which must not contain a white space, separated by white -// spaces and applies them to the given nodemap. -// -// An expection is thrown in case of an error. Execution stops on the first -// error. - -void applyParameters(const std::shared_ptr& nodemap, const std::string& parameters) -{ - size_t i = 0; - - while (i < parameters.size()) - { - // eat all white spaces - - while (i < parameters.size() && std::isspace(parameters[i])) - i++; - - // skip comments which start with # and end by \n - - if (i < parameters.size() && parameters[i] == '#') - { - while (i < parameters.size() && parameters[i] != '\n') - i++; - } - else - { - size_t k = i; - while (k < parameters.size() && !std::isspace(parameters[k])) - k++; - - size_t j = parameters.find('=', i); - - if (j <= k) - { - std::string name = parameters.substr(i, j - i); - std::string value = parameters.substr(j + 1, k - j - 1); - - rcg::setString(nodemap, name.c_str(), value.c_str(), true); - } - else if (i < k) - { - std::string name = parameters.substr(i, k - i); - - rcg::callCommand(nodemap, name.c_str(), true); - } - - i = k; - } - } -} - -} // namespace - -bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::srv::GetGenICamParameter::Request& req, - rc_genicam_camera::srv::GetGenICamParameter::Response& resp) -{ - std::lock_guard lock(device_mtx_); - - if (rcgnodemap_) - { - try - { - resp.value = rcg::getString(rcgnodemap_, req.name.c_str(), true); - resp.return_code.value = resp.return_code.SUCCESS; - resp.return_code.message = "ok"; - } - catch (const std::exception& ex) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot get parameter: " << ex.what()); - - resp.return_code.value = resp.return_code.INVALID_ARGUMENT; - resp.return_code.message = ex.what(); - } - } - - return true; -} - -bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera::srv::SetGenICamParameter::Request& req, - rc_genicam_camera::srv::SetGenICamParameter::Response& resp) -{ - std::lock_guard lock(device_mtx_); - - if (rcgnodemap_) - { - try - { - applyParameters(rcgnodemap_, req.parameters); - - resp.return_code.value = resp.return_code.SUCCESS; - resp.return_code.message = "ok"; - } - catch (const std::exception& ex) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot set parameters: " << ex.what()); - - resp.return_code.value = resp.return_code.INVALID_ARGUMENT; - resp.return_code.message = ex.what(); - } - } - - return true; -} - -namespace -{ - -void storeImage(const std::string &prefix, const sensor_msgs::msg::Image::SharedPtr image) -{ - // prepare file name - - std::ostringstream name; - - uint64_t t_sec = image->header.stamp.sec; - uint64_t t_nsec = image->header.stamp.nanosec; - - name << prefix << "_" << t_sec << "." << std::setfill('0') << std::setw(9) << t_nsec << ".pgm"; - - // store 8 bit images - - if (image->encoding == sensor_msgs::image_encodings::MONO8 || - image->encoding == sensor_msgs::image_encodings::BAYER_BGGR8 || - image->encoding == sensor_msgs::image_encodings::BAYER_GBRG8 || - image->encoding == sensor_msgs::image_encodings::BAYER_GRBG8 || - image->encoding == sensor_msgs::image_encodings::BAYER_RGGB8) - { - size_t width = image->width; - size_t height = image->height; - uint8_t* p = reinterpret_cast(&image->data[0]); - - FILE *out = fopen(name.str().c_str(), "w"); - - if (out) - { - fprintf(out, "P5\n%lu %lu\n255\n", width, height); - size_t n = fwrite(p, 1, width*height, out); - - if (n < width*height) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), - "Cannot write to file " << name.str() << " (" - << n << " < " << width*height << ")"); - } - - fclose(out); - } - else - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), - "Cannot create file " << name.str()); - } - } -} - -} - -void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info) -{ - sensor_msgs::msg::Image::SharedPtr image; - - { - std::lock_guard lock(sync_mtx_); - - // find image that corresponds to camera info - - image = image_list_.find(info->header.stamp); - - if (image != 0) - { - // remove all older images and infos - - int n = image_list_.removeOld(image->header.stamp) - 1; - info_list_.removeOld(info->header.stamp); - - if (n > 0) - { - RCLCPP_WARN_STREAM(this->get_logger(),"rc_genicam_camera: Dropped unused images: " << n); - } - - // correct time stamp of image - - image->header.stamp = info->header.stamp; - } - else - { - // store info - - info_list_.add(info); - } - } - - // publish images - - if (image) - { - caminfo_pub_.publish(image); - image_pub_.publish(image); - - // store images - - if (image_prefix_.size() > 0) - { - storeImage(image_prefix_, image); - } - } -} - -void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std::string config) -{ - try - { - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread started"); - - // load initial configuration for camera into string - - std::string init_params = loadConfig(config); - - // initialize optional timestamp correction - - TimestampCorrector ts_host; - ts_host.setMaximumTolerance(static_cast(timestamp_tolerance_ * 1000000000)); - ts_host.setInterval(1000000000ll); - - // loop until nodelet is killed - - while (running_) - { - // report standard exceptions and try again - - try - { - { - std::lock_guard lock(device_mtx_); - - // open device and get nodemap - - rcgdev_ = rcg::getDevice(device.c_str()); - - if (!rcgdev_) - { - throw std::invalid_argument("Cannot find device"); - } - - rcgdev_->open(access); - rcgnodemap_ = rcgdev_->getRemoteNodeMap(); - - // initialize camera - - try - { - applyParameters(rcgnodemap_, init_params); - } - catch (const std::exception& ex) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Error during initial camera configuration: " << ex.what()); - } - } - - // initialize timestamp correction - - if (!ts_host.determineOffset(rcgnodemap_)) - { - RCLCPP_ERROR_STREAM(this->get_logger(), - "rc_genicam_camera: Cannot determine offset between host and camera clock with maximum tolerance of " - << timestamp_tolerance_ << " s"); - } - - // start streaming - - std::vector > stream = rcgdev_->getStreams(); - - if (stream.size() == 0) - { - throw std::invalid_argument("Device does not offer streams"); - } - - stream[0]->open(); - stream[0]->startStreaming(); - - RCLCPP_INFO_STREAM(this->get_logger(), "rc_genicam_camera: Start streaming"); - - // grabbing thread - - while (running_) - { - const rcg::Buffer* buffer = stream[0]->grab(50); - - if (buffer != 0) - { - if (!buffer->getIsIncomplete()) - { - uint32_t npart = buffer->getNumberOfParts(); - for (uint32_t part = 0; part < npart; part++) - { - if (buffer->getImagePresent(part)) - { - // convert image to ROS - - sensor_msgs::msg::Image::SharedPtr image = rosImageFromBuffer(frame_id_, buffer, part, rotate_); - - if (image) - { - // correct timestamp - ts_host.correct(image->header.stamp); - - // optionally take timestamp of approximately synchronized - // camera info - - if (sync_tolerance_ > 0) - { - std::lock_guard lock(sync_mtx_); - - // find camera info that corresponds to the image - - sensor_msgs::msg::CameraInfo::SharedPtr info = info_list_.find(image->header.stamp); - - if (info != 0) - { - // remove all older images and infos - - int n = image_list_.removeOld(image->header.stamp); - info_list_.removeOld(info->header.stamp); - - if (n > 0) - { - RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Dropped images: " << n); - } - - // correct time stamp of image - - image->header.stamp = info->header.stamp; - } - else - { - // store image in internal list for later - // synchronization to camera info - - image = image_list_.add(image); - - if (image) - { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "rc_genicam_camera: Input queue full, dropping image"); - } - - image.reset(); - } - } - - // publish images - - if (image) - { - caminfo_pub_.publish(image); - image_pub_.publish(image); - - // store images - - if (image_prefix_.size() > 0) - { - storeImage(image_prefix_, image); - } - } - } - else - { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Unsupported pixel format"); - } - } - } - - // re-determine offset of host and camera clock - - if (!ts_host.determineOffset(rcgnodemap_)) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot determine offset between host and camera clock with " - "maximum tolerance of " - << timestamp_tolerance_ << " s"); - } - } - else - { - RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Incomplete buffer received"); - } - } - } - - // stop streaming - - stream[0]->stopStreaming(); - stream[0]->close(); - } - catch (const std::exception& ex) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - // close device - - { - std::lock_guard lock(device_mtx_); - - if (rcgdev_) - rcgdev_->close(); - - rcgdev_.reset(); - rcgnodemap_.reset(); - } - } - } - catch (const std::exception& ex) - { - RCLCPP_FATAL_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); - } - catch (...) - { - RCLCPP_FATAL_STREAM(this->get_logger(),"rc_genicam_camera: Unknown exception"); - } - - running_ = false; - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread stopped"); -} - -} // namespace rcgccam - -PLUGINLIB_EXPORT_CLASS(rcgccam::GenICamCameraNode, rclcpp::Node) From 5fb2c3fa39ab29c6e8199d9851ad9ced88bbc796 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Thu, 25 Aug 2022 23:58:51 +0100 Subject: [PATCH 18/32] removed ros1 steadytime check --- src/genicam_camera_node.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/genicam_camera_node.cc b/src/genicam_camera_node.cc index 896183d..2d95325 100644 --- a/src/genicam_camera_node.cc +++ b/src/genicam_camera_node.cc @@ -56,8 +56,6 @@ namespace rcgccam { -// TODO check this -#define ROS_HAS_STEADYTIME (ROS_VERSION_MINIMUM(1, 13, 1) || ((ROS_VERSION_MINOR == 12) && ROS_VERSION_PATCH >= 8)) explicit GenICamCameraNode::GenICamCameraNode(const std::string& node_name) : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) From 0c6d1ddbd42ba61436f9a27a6fabdc860ddb651f Mon Sep 17 00:00:00 2001 From: MRo47 Date: Fri, 26 Aug 2022 00:11:22 +0100 Subject: [PATCH 19/32] updated node main --- src/genicam_camera_node_main.cc | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/genicam_camera_node_main.cc b/src/genicam_camera_node_main.cc index 05f821a..63de958 100644 --- a/src/genicam_camera_node_main.cc +++ b/src/genicam_camera_node_main.cc @@ -31,19 +31,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include +#include "genicam_camera_node.h" int main(int argc, char** argv) { - ros::init(argc, argv, "rc_genicam_camera"); + rclcpp::init(argc, argv); + auto genicam_camera_node = std::make_shared( + "rc_genicam_camera_node"); - nodelet::Loader manager(false); - nodelet::M_string remappings(ros::names::getRemappings()); - nodelet::V_string my_argv(argv + 1, argv + argc); + rclcpp::spin(genicam_camera_node); - manager.load(ros::this_node::getName(), "rc_genicam_camera", remappings, my_argv); - - ros::spin(); + rclcpp::shutdown(); return 0; } From e0cb531396533ec46118233a2218e9f00852e7cd Mon Sep 17 00:00:00 2001 From: MRo47 Date: Fri, 26 Aug 2022 00:26:12 +0100 Subject: [PATCH 20/32] remove nested cmakelists --- src/CMakeLists.txt | 29 ----------------------------- 1 file changed, 29 deletions(-) delete mode 100644 src/CMakeLists.txt diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index 0362027..0000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ - -set(SRC - genicam_camera_nodelet.cc - publishers/camera_info_publisher.cc - publishers/image_publisher.cc - timestamp_corrector.cc - imagelist.cc - camerainfolist.cc) - -# build and install the nodelet - -add_library(rc_genicam_camera_nodelet ${SRC}) - -target_link_libraries(rc_genicam_camera_nodelet - PUBLIC - ${catkin_LIBRARIES} - PRIVATE - rc_genicam_api::rc_genicam_api -) - -add_executable(rc_genicam_camera genicam_camera_node.cc) -target_link_libraries(rc_genicam_camera ${catkin_LIBRARIES}) - -install(TARGETS rc_genicam_camera rc_genicam_camera_nodelet - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -#add_dependencies(rc_genicam_camera_nodelet rc_genicam_camera_gencfg) From ff047213021b0eadcb4db0ec8f77fd3e604c65fe Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 00:51:38 +0100 Subject: [PATCH 21/32] corrected header path --- src/publishers/camera_info_publisher.cc | 2 +- src/publishers/image_publisher.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/publishers/camera_info_publisher.cc b/src/publishers/camera_info_publisher.cc index 3b6fb1c..0033483 100644 --- a/src/publishers/camera_info_publisher.cc +++ b/src/publishers/camera_info_publisher.cc @@ -31,7 +31,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "camera_info_publisher.h" +#include "publishers/camera_info_publisher.h" #include #include diff --git a/src/publishers/image_publisher.cc b/src/publishers/image_publisher.cc index 00b31a7..dc080db 100644 --- a/src/publishers/image_publisher.cc +++ b/src/publishers/image_publisher.cc @@ -31,7 +31,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "image_publisher.h" +#include "publishers/image_publisher.h" #include #include From 402cc2632656240d0f337df93a553bbb69b5ed50 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 00:52:28 +0100 Subject: [PATCH 22/32] pcakage xml remove builtin interface depend --- package.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/package.xml b/package.xml index 0bcbf92..52832f2 100644 --- a/package.xml +++ b/package.xml @@ -18,11 +18,8 @@ ament_cmake - builtin_interfaces rosidl_default_generators - builtin_interfaces rosidl_default_runtime - rclcpp std_srvs From fb01377d168609869c01fc8772e23835c0ab1bf2 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 14:36:52 +0100 Subject: [PATCH 23/32] moved interfaces out --- CMakeLists.txt | 194 +++++++++++++++------------ include/genicam_camera_node.h | 16 +-- include/publishers/image_publisher.h | 2 +- msg/ReturnValue.msg | 23 ---- package.xml | 5 +- src/genicam_camera_node.cc | 16 +-- srv/GetGenICamParameter.srv | 12 -- srv/SetGenICamParameter.srv | 9 -- 8 files changed, 124 insertions(+), 153 deletions(-) delete mode 100644 msg/ReturnValue.msg delete mode 100644 srv/GetGenICamParameter.srv delete mode 100644 srv/SetGenICamParameter.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 64ac3b1..471c8e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(rc_genicam_camera) include(cmake/project_version.cmake) @@ -9,17 +9,14 @@ if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE RELEASE CACHE STRING "Build type: DEBUG or RELEASE" FORCE) endif () -# - Standard definitions - +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -add_definitions(-Wall) -if (CMAKE_MAJOR_VERSION VERSION_LESS "3.1.0") - add_definitions(-std=c++11) -else () - set(CMAKE_CXX_STANDARD 11) -endif () - -add_definitions(-Wall) -add_definitions(-Wno-unknown-pragmas) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() # checking for intel architecture and activating SSSE3 for optimization of # rotating images @@ -37,97 +34,118 @@ if (USE_SSSE3) add_definitions(-mssse3) endif () -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - nodelet - roscpp - std_srvs - sensor_msgs - image_transport - dynamic_reconfigure - message_generation -) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rc_genicam_camera_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +# find_package(std_msgs REQUIRED) +# find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(RC_GENICAM_API REQUIRED PATHS "/home/myron/personal/iye/libraries/rc_genicam_api") + +# rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/ReturnValue.msg" +# "srv/GetGenICamParameter.srv" +# "srv/SetGenICamParameter.srv" +# # DEPENDENCIES builtin_interfaces +# ) + +# ament_export_dependencies(rosidl_default_runtime) -## Generate services in the 'srv' folder -add_service_files( - FILES - GetGenICamParameter.srv - SetGenICamParameter.srv +include_directories( + include + ${RC_GENICAM_API_INCLUDE_DIRS} ) -## Generate messages in the 'msg' folder -add_message_files( - FILES - ReturnValue.msg +ament_export_include_directories("include/${PROJECT_NAME}") + +# add_library(${PROJECT_NAME}_libs +# src/publishers/camera_info_publisher.cc +# src/publishers/image_publisher.cc +# src/camerainfolist.cc +# src/genicam_camera_node.cc +# src/imagelist.cc +# src/timestamp_corrector.cc +# ) + +# target_link_libraries(${PROJECT_NAME}_libs +# # PUBLIC +# # rclcpp::rclcpp +# # ${builtin_interfaces_TARGETS} +# # ${sensor_msgs_TARGETS} +# # PRIVATE +# # ${RC_GENICAM_API_LIBRARIES} +# rc_genicam_api::rc_genicam_api +# ) + +# ament_target_dependencies(${PROJECT_NAME}_libs +# "rclcpp" +# "builtin_interfaces" +# "sensor_msgs" +# # ${PROJECT_NAME} +# ) + +# target_link_libraries(${PROJECT_NAME}_libs +# rc_genicam_api::rc_genicam_api +# ) + +add_executable(${PROJECT_NAME}_node + src/genicam_camera_node_main.cc + src/publishers/camera_info_publisher.cc + src/publishers/image_publisher.cc + src/camerainfolist.cc + src/genicam_camera_node.cc + src/imagelist.cc + src/timestamp_corrector.cc ) -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - sensor_msgs - std_msgs +ament_target_dependencies(${PROJECT_NAME}_node + "rclcpp" + "sensor_msgs" + "rc_genicam_camera_interfaces" ) -## System dependencies are found with CMake's conventions -find_package(RC_GENICAM_API 2.0.3 REQUIRED) - -######################################### -## Definitions for dynamic reconfigure ## -######################################### - -#generate_dynamic_reconfigure_options(cfg/rc_genicam_camera.cfg) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES rc_stereocamera - CATKIN_DEPENDS nodelet roscpp sensor_msgs std_msgs std_srvs -# DEPENDS +target_link_libraries(${PROJECT_NAME}_node + rc_genicam_api::rc_genicam_api ) -########### -## Build ## -########### +install(TARGETS + ${PROJECT_NAME}_node + # ${PROJECT_NAME}_libs + DESTINATION lib/${PROJECT_NAME} +) -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ${catkin_INCLUDE_DIRS} - ${RC_GENICAM_API_INCLUDE_DIRS} +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ ) -add_subdirectory(src) +# add_subdirectory(src) -############# -## Install ## -############# +# ############ +# # Install ## +# ############ # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - rc_genicam_camera_nodelet.xml - README.md - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) -#install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -###################################### -## Define information for packaging ## -###################################### -# optionally specify dependencies of the debian package here (comma separated!) -set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS OFF) # to prevent automatic addition of libs that are ORed in dependencies -set(CPACK_DEBIAN_PACKAGE_DEPENDS "rc-genicam-api (>= 2.0.3) | ros-$ENV{ROS_DISTRO}-rc-genicam-api (>= 2.0.3), ros-$ENV{ROS_DISTRO}-image-pipeline") - -include(cmake/package_debian.cmake) +# ## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# rc_genicam_camera_nodelet.xml +# README.md +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) +# #install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# ###################################### +# ## Define information for packaging ## +# ###################################### +# # optionally specify dependencies of the debian package here (comma separated!) +# set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS OFF) # to prevent automatic addition of libs that are ORed in dependencies +# set(CPACK_DEBIAN_PACKAGE_DEPENDS "rc-genicam-api (>= 2.0.3) | ros-$ENV{ROS_DISTRO}-rc-genicam-api (>= 2.0.3), ros-$ENV{ROS_DISTRO}-image-pipeline") + +# include(cmake/package_debian.cmake) + +ament_package() diff --git a/include/genicam_camera_node.h b/include/genicam_camera_node.h index b92fa13..e12d954 100644 --- a/include/genicam_camera_node.h +++ b/include/genicam_camera_node.h @@ -50,8 +50,8 @@ #include #include -#include -#include +#include +#include namespace rcgccam { @@ -65,11 +65,11 @@ class GenICamCameraNode : public rclcpp::Node virtual void onInit(); - bool getGenICamParameter(rc_genicam_camera::srv::GetGenICamParameter::Request& req, - rc_genicam_camera::srv::GetGenICamParameter::Response& resp); + bool getGenICamParameter(rc_genicam_camera_interfaces::srv::GetGenICamParameter::Request& req, + rc_genicam_camera_interfaces::srv::GetGenICamParameter::Response& resp); - bool setGenICamParameter(rc_genicam_camera::srv::SetGenICamParameter::Request& req, - rc_genicam_camera::srv::SetGenICamParameter::Response& resp); + bool setGenICamParameter(rc_genicam_camera_interfaces::srv::SetGenICamParameter::Request& req, + rc_genicam_camera_interfaces::srv::SetGenICamParameter::Response& resp); void syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info); @@ -82,8 +82,8 @@ class GenICamCameraNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_sync_info_ptr_; - rclcpp::Service::SharedPtr get_param_service_ptr_; - rclcpp::Service::SharedPtr set_param_service_ptr_; + rclcpp::Service::SharedPtr get_param_service_ptr_; + rclcpp::Service::SharedPtr set_param_service_ptr_; std::string frame_id_; diff --git a/include/publishers/image_publisher.h b/include/publishers/image_publisher.h index afac518..00784b7 100644 --- a/include/publishers/image_publisher.h +++ b/include/publishers/image_publisher.h @@ -35,7 +35,7 @@ #define RCGCCAM_IMAGEPUBLISHER_H #include -#include +#include #include #include diff --git a/msg/ReturnValue.msg b/msg/ReturnValue.msg deleted file mode 100644 index 9c7592f..0000000 --- a/msg/ReturnValue.msg +++ /dev/null @@ -1,23 +0,0 @@ -# return codes -# >=0 - success -# < 0 - failure - -# return code -int16 value - -# return message -string message - -# Default success without additional information -int16 SUCCESS=0 - -# Reserved error codes -int16 INVALID_ARGUMENT=-1 -int16 UNINITIALIZED=-2 -int16 INTERNAL_TIMEOUT=-3 -int16 SENSOR_TIMEOUT=-4 -int16 REQUEST_TO_DATABASE_FAILED=-5 -int16 REQUEST_TO_TF_MODULE_FAILED=-6 -int16 IO_ERROR=-7 -int16 INVALID_REQUEST=-8 # Request not applicable (e.g. in current state) -int16 INVALID_LICENSE=-9 diff --git a/package.xml b/package.xml index 52832f2..393bc66 100644 --- a/package.xml +++ b/package.xml @@ -22,15 +22,12 @@ rosidl_default_runtime rclcpp - std_srvs - std_msgs sensor_msgs image_transport + rc_genicam_camera_interfaces - - rosidl_interface_packages doxygen diff --git a/src/genicam_camera_node.cc b/src/genicam_camera_node.cc index 2d95325..7e5c678 100644 --- a/src/genicam_camera_node.cc +++ b/src/genicam_camera_node.cc @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include @@ -57,7 +57,7 @@ namespace rcgccam { -explicit GenICamCameraNode::GenICamCameraNode(const std::string& node_name) +GenICamCameraNode::GenICamCameraNode(const std::string& node_name) : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) { timestamp_tolerance_ = 0; @@ -197,12 +197,12 @@ void GenICamCameraNode::onInit() } // setup service for getting and setting parameters - get_param_service_ptr_ = this->create_service( + get_param_service_ptr_ = this->create_service( "get_genicam_parameter", std::bind(&GenICamCameraNode::getGenICamParameter, this, std::placeholders::_1) ); - set_param_service_ptr_ = this->create_service( + set_param_service_ptr_ = this->create_service( "set_genicam_parameter", std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1) ); @@ -304,8 +304,8 @@ void applyParameters(const std::shared_ptr& nodemap, const } // namespace -bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::srv::GetGenICamParameter::Request& req, - rc_genicam_camera::srv::GetGenICamParameter::Response& resp) +bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera_interfaces::srv::GetGenICamParameter::Request& req, + rc_genicam_camera_interfaces::srv::GetGenICamParameter::Response& resp) { std::lock_guard lock(device_mtx_); @@ -329,8 +329,8 @@ bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera::srv::GetGenICamPa return true; } -bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera::srv::SetGenICamParameter::Request& req, - rc_genicam_camera::srv::SetGenICamParameter::Response& resp) +bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera_interfaces::srv::SetGenICamParameter::Request& req, + rc_genicam_camera_interfaces::srv::SetGenICamParameter::Response& resp) { std::lock_guard lock(device_mtx_); diff --git a/srv/GetGenICamParameter.srv b/srv/GetGenICamParameter.srv deleted file mode 100644 index 687a863..0000000 --- a/srv/GetGenICamParameter.srv +++ /dev/null @@ -1,12 +0,0 @@ -# Description: Get the value of a GenICam parameter. - -# Name of the parameter -string name - ---- - -# Value of the parameter converted to a string -string value - -# Return code -rc_genicam_camera/ReturnValue return_code diff --git a/srv/SetGenICamParameter.srv b/srv/SetGenICamParameter.srv deleted file mode 100644 index 29974b4..0000000 --- a/srv/SetGenICamParameter.srv +++ /dev/null @@ -1,9 +0,0 @@ -# Description: Set the values of GenICam parameters. - -# One or more GenICam = pair, separated by spaces -string parameters - ---- - -# Return code -rc_genicam_camera/ReturnValue return_code From f68df4e8d83d4bb9c04db2a1c1a5ae3e55a54adc Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 17:10:55 +0100 Subject: [PATCH 24/32] added service args as ptrs + compiled successfully --- CMakeLists.txt | 45 +---------------------------------- include/genicam_camera_node.h | 14 ++++++----- src/genicam_camera_node.cc | 38 ++++++++++++++--------------- 3 files changed, 27 insertions(+), 70 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 471c8e1..a50cbe3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,21 +38,10 @@ find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rc_genicam_camera_interfaces REQUIRED) find_package(rclcpp REQUIRED) -# find_package(std_msgs REQUIRED) -# find_package(std_srvs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(image_transport REQUIRED) find_package(RC_GENICAM_API REQUIRED PATHS "/home/myron/personal/iye/libraries/rc_genicam_api") -# rosidl_generate_interfaces(${PROJECT_NAME} -# "msg/ReturnValue.msg" -# "srv/GetGenICamParameter.srv" -# "srv/SetGenICamParameter.srv" -# # DEPENDENCIES builtin_interfaces -# ) - -# ament_export_dependencies(rosidl_default_runtime) - include_directories( include ${RC_GENICAM_API_INCLUDE_DIRS} @@ -60,36 +49,6 @@ include_directories( ament_export_include_directories("include/${PROJECT_NAME}") -# add_library(${PROJECT_NAME}_libs -# src/publishers/camera_info_publisher.cc -# src/publishers/image_publisher.cc -# src/camerainfolist.cc -# src/genicam_camera_node.cc -# src/imagelist.cc -# src/timestamp_corrector.cc -# ) - -# target_link_libraries(${PROJECT_NAME}_libs -# # PUBLIC -# # rclcpp::rclcpp -# # ${builtin_interfaces_TARGETS} -# # ${sensor_msgs_TARGETS} -# # PRIVATE -# # ${RC_GENICAM_API_LIBRARIES} -# rc_genicam_api::rc_genicam_api -# ) - -# ament_target_dependencies(${PROJECT_NAME}_libs -# "rclcpp" -# "builtin_interfaces" -# "sensor_msgs" -# # ${PROJECT_NAME} -# ) - -# target_link_libraries(${PROJECT_NAME}_libs -# rc_genicam_api::rc_genicam_api -# ) - add_executable(${PROJECT_NAME}_node src/genicam_camera_node_main.cc src/publishers/camera_info_publisher.cc @@ -103,6 +62,7 @@ add_executable(${PROJECT_NAME}_node ament_target_dependencies(${PROJECT_NAME}_node "rclcpp" "sensor_msgs" + "image_transport" "rc_genicam_camera_interfaces" ) @@ -112,7 +72,6 @@ target_link_libraries(${PROJECT_NAME}_node install(TARGETS ${PROJECT_NAME}_node - # ${PROJECT_NAME}_libs DESTINATION lib/${PROJECT_NAME} ) @@ -122,8 +81,6 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) -# add_subdirectory(src) - # ############ # # Install ## # ############ diff --git a/include/genicam_camera_node.h b/include/genicam_camera_node.h index e12d954..63847b3 100644 --- a/include/genicam_camera_node.h +++ b/include/genicam_camera_node.h @@ -58,6 +58,8 @@ namespace rcgccam class GenICamCameraNode : public rclcpp::Node { + using rcgc_get_srv = rc_genicam_camera_interfaces::srv::GetGenICamParameter; + using rcgc_set_srv = rc_genicam_camera_interfaces::srv::SetGenICamParameter; public: explicit GenICamCameraNode(const std::string& node_name = "rc_genicam_camera_node"); @@ -65,11 +67,11 @@ class GenICamCameraNode : public rclcpp::Node virtual void onInit(); - bool getGenICamParameter(rc_genicam_camera_interfaces::srv::GetGenICamParameter::Request& req, - rc_genicam_camera_interfaces::srv::GetGenICamParameter::Response& resp); + bool getGenICamParameter(rcgc_get_srv::Request::SharedPtr req, + rcgc_get_srv::Response::SharedPtr resp); - bool setGenICamParameter(rc_genicam_camera_interfaces::srv::SetGenICamParameter::Request& req, - rc_genicam_camera_interfaces::srv::SetGenICamParameter::Response& resp); + bool setGenICamParameter(rcgc_set_srv::Request::SharedPtr req, + rcgc_set_srv::Response::SharedPtr resp); void syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info); @@ -82,8 +84,8 @@ class GenICamCameraNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_sync_info_ptr_; - rclcpp::Service::SharedPtr get_param_service_ptr_; - rclcpp::Service::SharedPtr set_param_service_ptr_; + rclcpp::Service::SharedPtr get_param_service_ptr_; + rclcpp::Service::SharedPtr set_param_service_ptr_; std::string frame_id_; diff --git a/src/genicam_camera_node.cc b/src/genicam_camera_node.cc index 7e5c678..a857864 100644 --- a/src/genicam_camera_node.cc +++ b/src/genicam_camera_node.cc @@ -198,14 +198,12 @@ void GenICamCameraNode::onInit() // setup service for getting and setting parameters get_param_service_ptr_ = this->create_service( - "get_genicam_parameter", - std::bind(&GenICamCameraNode::getGenICamParameter, this, std::placeholders::_1) - ); + "get_genicam_parameter", + std::bind(&GenICamCameraNode::getGenICamParameter, this, std::placeholders::_1, std::placeholders::_2)); set_param_service_ptr_ = this->create_service( - "set_genicam_parameter", - std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1) - ); + "set_genicam_parameter", + std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1, std::placeholders::_2)); // initialize publishers caminfo_pub_.init(this->shared_from_this(), calib.c_str(), calib_id); @@ -304,8 +302,8 @@ void applyParameters(const std::shared_ptr& nodemap, const } // namespace -bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera_interfaces::srv::GetGenICamParameter::Request& req, - rc_genicam_camera_interfaces::srv::GetGenICamParameter::Response& resp) +bool GenICamCameraNode::getGenICamParameter(rcgc_get_srv::Request::SharedPtr req, + rcgc_get_srv::Response::SharedPtr resp) { std::lock_guard lock(device_mtx_); @@ -313,24 +311,24 @@ bool GenICamCameraNode::getGenICamParameter(rc_genicam_camera_interfaces::srv::G { try { - resp.value = rcg::getString(rcgnodemap_, req.name.c_str(), true); - resp.return_code.value = resp.return_code.SUCCESS; - resp.return_code.message = "ok"; + resp->value = rcg::getString(rcgnodemap_, req->name.c_str(), true); + resp->return_code.value = resp->return_code.SUCCESS; + resp->return_code.message = "ok"; } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot get parameter: " << ex.what()); - resp.return_code.value = resp.return_code.INVALID_ARGUMENT; - resp.return_code.message = ex.what(); + resp->return_code.value = resp->return_code.INVALID_ARGUMENT; + resp->return_code.message = ex.what(); } } return true; } -bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera_interfaces::srv::SetGenICamParameter::Request& req, - rc_genicam_camera_interfaces::srv::SetGenICamParameter::Response& resp) +bool GenICamCameraNode::setGenICamParameter(rcgc_set_srv::Request::SharedPtr req, + rcgc_set_srv::Response::SharedPtr resp) { std::lock_guard lock(device_mtx_); @@ -338,17 +336,17 @@ bool GenICamCameraNode::setGenICamParameter(rc_genicam_camera_interfaces::srv::S { try { - applyParameters(rcgnodemap_, req.parameters); + applyParameters(rcgnodemap_, req->parameters); - resp.return_code.value = resp.return_code.SUCCESS; - resp.return_code.message = "ok"; + resp->return_code.value = resp->return_code.SUCCESS; + resp->return_code.message = "ok"; } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot set parameters: " << ex.what()); - resp.return_code.value = resp.return_code.INVALID_ARGUMENT; - resp.return_code.message = ex.what(); + resp->return_code.value = resp->return_code.INVALID_ARGUMENT; + resp->return_code.message = ex.what(); } } From f7ec9ca4177a779becd3c6cdc35a93110f0a2b57 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 23:11:44 +0100 Subject: [PATCH 25/32] fixed cmakelists able to run --- CMakeLists.txt | 30 ++++++++++++++---------------- package.xml | 8 ++++---- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a50cbe3..086679d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,7 @@ find_package(rc_genicam_camera_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(image_transport REQUIRED) -find_package(RC_GENICAM_API REQUIRED PATHS "/home/myron/personal/iye/libraries/rc_genicam_api") +find_package(RC_GENICAM_API REQUIRED) include_directories( include @@ -49,7 +49,7 @@ include_directories( ament_export_include_directories("include/${PROJECT_NAME}") -add_executable(${PROJECT_NAME}_node +add_executable(${PROJECT_NAME} src/genicam_camera_node_main.cc src/publishers/camera_info_publisher.cc src/publishers/image_publisher.cc @@ -59,32 +59,30 @@ add_executable(${PROJECT_NAME}_node src/timestamp_corrector.cc ) -ament_target_dependencies(${PROJECT_NAME}_node - "rclcpp" - "sensor_msgs" - "image_transport" - "rc_genicam_camera_interfaces" +ament_target_dependencies(${PROJECT_NAME} + rclcpp + sensor_msgs + image_transport + rc_genicam_camera_interfaces ) -target_link_libraries(${PROJECT_NAME}_node +target_link_libraries(${PROJECT_NAME} rc_genicam_api::rc_genicam_api ) -install(TARGETS - ${PROJECT_NAME}_node +# ############ +# # Install ## +# ############ + +install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) # Install launch files. -install(DIRECTORY - launch +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) -# ############ -# # Install ## -# ############ - # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html diff --git a/package.xml b/package.xml index 393bc66..e4a49b9 100644 --- a/package.xml +++ b/package.xml @@ -13,14 +13,14 @@ Heiko Hirschmueller - - rc_genicam_api - ament_cmake rosidl_default_generators rosidl_default_runtime - + + + + rc_genicam_api rclcpp sensor_msgs image_transport From fb47e3a27b20b48fc8def67df6499f30447fe5c1 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 23:52:09 +0100 Subject: [PATCH 26/32] name_[node] for exec --- CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 086679d..8fc90a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,7 @@ include_directories( ament_export_include_directories("include/${PROJECT_NAME}") -add_executable(${PROJECT_NAME} +add_executable(${PROJECT_NAME}_node src/genicam_camera_node_main.cc src/publishers/camera_info_publisher.cc src/publishers/image_publisher.cc @@ -59,14 +59,14 @@ add_executable(${PROJECT_NAME} src/timestamp_corrector.cc ) -ament_target_dependencies(${PROJECT_NAME} +ament_target_dependencies(${PROJECT_NAME}_node rclcpp sensor_msgs image_transport rc_genicam_camera_interfaces ) -target_link_libraries(${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME}_node rc_genicam_api::rc_genicam_api ) @@ -74,7 +74,7 @@ target_link_libraries(${PROJECT_NAME} # # Install ## # ############ -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME} ) From 63284b7223111f8830f1605760827d1ea6eb183a Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 23:52:27 +0100 Subject: [PATCH 27/32] added launch and config --- config/rc_genicam_camera_params.yaml | 14 ++++++++++++++ launch/rc_genicam_camera_launch.py | 25 +++++++++++++++++++++++++ 2 files changed, 39 insertions(+) create mode 100644 config/rc_genicam_camera_params.yaml create mode 100644 launch/rc_genicam_camera_launch.py diff --git a/config/rc_genicam_camera_params.yaml b/config/rc_genicam_camera_params.yaml new file mode 100644 index 0000000..1640eb0 --- /dev/null +++ b/config/rc_genicam_camera_params.yaml @@ -0,0 +1,14 @@ +rc_genicam_camera_node: + ros__parameters: + device: + gev_access: + frame_id: + image_prefix: + rotate: + config_file: + calib_file: + calib_id: + host_timestamp: + timestamp_tolerance: + sync_info: + sync_tolderance: \ No newline at end of file diff --git a/launch/rc_genicam_camera_launch.py b/launch/rc_genicam_camera_launch.py new file mode 100644 index 0000000..760c145 --- /dev/null +++ b/launch/rc_genicam_camera_launch.py @@ -0,0 +1,25 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + config = os.path.join( + get_package_share_directory('rc_genicam_camera'), + 'config', + 'rc_genicam_camera_params.yaml' + ) + + return LaunchDescription([ + Node( + package='rc_genicam_camera', + # namespace='rc_genicam', + executable='rc_genicam_camera_node', + name='rc_genicam_camera_node', + # remappings=[ + # ('/image', '/image/raw'), + # ] + parameters = [config] + ) + ]) \ No newline at end of file From 3f71fdd611e5dcecb0e97301da34cf5a9a811071 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Aug 2022 23:52:48 +0100 Subject: [PATCH 28/32] readme update for rosdep install --- README.md | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 52912e1..dcc2f8e 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,18 @@ -rc_genicam_camera ------------------ +# rc_genicam_camera This nodelet permits access to the configuration of a GenICam compatible camera and publishes raw images and camera_info messages according to the ROS image pipeline. -Configuration -------------- +## Install dependencies +```bash +rosdep update +rosdep install --from-paths src -y --ignore-src +``` + +## Configuration -#### Parameters +### Parameters Parameters to be set to the ROS param server before run-time. @@ -95,7 +99,7 @@ Parameters to be set to the ROS param server before run-time. ID < 0. For stereo systems, 0 is used for the left camera and 1 for the right camera. Default: -1. -#### Parameters for Synchronization +### Parameters for Synchronization The following parameters are used for receiving and associating the time stamp from the camera info of a hardware synchronized master camera that is From 96d9ccb8f039b58d12b994b17dd4ff049e54cf66 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sat, 3 Sep 2022 15:58:54 +0100 Subject: [PATCH 29/32] run on init for node --- src/genicam_camera_node_main.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/genicam_camera_node_main.cc b/src/genicam_camera_node_main.cc index 63de958..5d2b5f5 100644 --- a/src/genicam_camera_node_main.cc +++ b/src/genicam_camera_node_main.cc @@ -40,6 +40,8 @@ int main(int argc, char** argv) auto genicam_camera_node = std::make_shared( "rc_genicam_camera_node"); + genicam_camera_node->onInit(); + rclcpp::spin(genicam_camera_node); rclcpp::shutdown(); From ea379238fafe8e4f8ffe56de1d341324de527051 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sat, 3 Sep 2022 16:01:30 +0100 Subject: [PATCH 30/32] cleanup logging + more messages in grab --- src/genicam_camera_node.cc | 82 +++++++++++++++++++++++--------------- 1 file changed, 50 insertions(+), 32 deletions(-) diff --git a/src/genicam_camera_node.cc b/src/genicam_camera_node.cc index a857864..9a02d52 100644 --- a/src/genicam_camera_node.cc +++ b/src/genicam_camera_node.cc @@ -65,23 +65,24 @@ GenICamCameraNode::GenICamCameraNode(const std::string& node_name) rotate_ = false; running_ = false; - this->declare_parameter("frame_id"); + this->declare_parameter("frame_id", ""); this->declare_parameter("device"); this->declare_parameter("gev_access"); - this->declare_parameter("config_file"); - this->declare_parameter("calib_file"); - this->declare_parameter("calib_id"); + this->declare_parameter("config_file", ""); + this->declare_parameter("calib_file", ""); + this->declare_parameter("calib_id", -1); this->declare_parameter("host_timestamp", false); this->declare_parameter("timestamp_tolerance_", 0.01); - this->declare_parameter("sync_info"); + this->declare_parameter("sync_info", ""); this->declare_parameter("sync_tolerance", 0.019); - this->declare_parameter("image_prefix"); - this->declare_parameter("rotate"); + this->declare_parameter("image_prefix", ""); + this->declare_parameter("rotate", false); + } GenICamCameraNode::~GenICamCameraNode() { - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Shutting down"); + RCLCPP_INFO(this->get_logger(), "Shutting down"); // signal running_ threads and wait until they finish @@ -96,7 +97,7 @@ GenICamCameraNode::~GenICamCameraNode() void GenICamCameraNode::onInit() { - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Initialization"); + RCLCPP_INFO(this->get_logger(), "Initialization started"); // get parameter configuration @@ -158,7 +159,7 @@ void GenICamCameraNode::onInit() } else { - RCLCPP_FATAL_STREAM(this->get_logger(), "rc_visard_driver: Access must be 'control' or 'exclusive': " << access); + RCLCPP_FATAL_STREAM(this->get_logger(), "Access must be 'control' or 'exclusive': " << access); return; } @@ -204,10 +205,10 @@ void GenICamCameraNode::onInit() set_param_service_ptr_ = this->create_service( "set_genicam_parameter", std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1, std::placeholders::_2)); - + // initialize publishers caminfo_pub_.init(this->shared_from_this(), calib.c_str(), calib_id); - + image_transport::ImageTransport it(this->shared_from_this()); image_pub_.init(it); @@ -220,6 +221,8 @@ void GenICamCameraNode::onInit() // start grabbing threads running_ = true; grab_thread_ = std::thread(&GenICamCameraNode::grab, this, device, access_id, config); + + RCLCPP_INFO(this->get_logger(), "Initialization complete"); } namespace @@ -230,6 +233,8 @@ std::string loadConfig(const std::string& filename) { if (filename.size() > 0) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), + "Reading camera config file: " << filename); std::ifstream in(filename); std::stringstream buffer; @@ -240,8 +245,8 @@ std::string loadConfig(const std::string& filename) } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::loadConfig"), - "rc_genicam_camera: Cannot load config: " << filename); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), + "Cannot load config: " << filename); } } @@ -257,6 +262,11 @@ std::string loadConfig(const std::string& filename) void applyParameters(const std::shared_ptr& nodemap, const std::string& parameters) { + if(parameters.size() > 0) + { + RCLCPP_INFO_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), + "Configuring camera"); + } size_t i = 0; while (i < parameters.size()) @@ -317,7 +327,7 @@ bool GenICamCameraNode::getGenICamParameter(rcgc_get_srv::Request::SharedPtr req } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot get parameter: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot get parameter: " << ex.what()); resp->return_code.value = resp->return_code.INVALID_ARGUMENT; resp->return_code.message = ex.what(); @@ -343,7 +353,7 @@ bool GenICamCameraNode::setGenICamParameter(rcgc_set_srv::Request::SharedPtr req } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot set parameters: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot set parameters: " << ex.what()); resp->return_code.value = resp->return_code.INVALID_ARGUMENT; resp->return_code.message = ex.what(); @@ -388,7 +398,7 @@ void storeImage(const std::string &prefix, const sensor_msgs::msg::Image::Shared if (n < width*height) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), + RCLCPP_ERROR_STREAM(rclcpp::get_logger("storeImage"), "Cannot write to file " << name.str() << " (" << n << " < " << width*height << ")"); } @@ -397,7 +407,7 @@ void storeImage(const std::string &prefix, const sensor_msgs::msg::Image::Shared } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera::storeImage"), + RCLCPP_ERROR_STREAM(rclcpp::get_logger("storeImage"), "Cannot create file " << name.str()); } } @@ -425,7 +435,7 @@ void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info) if (n > 0) { - RCLCPP_WARN_STREAM(this->get_logger(),"rc_genicam_camera: Dropped unused images: " << n); + RCLCPP_WARN_STREAM(this->get_logger()," Dropped unused images: " << n); } // correct time stamp of image @@ -460,7 +470,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std { try { - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread started"); + RCLCPP_INFO(this->get_logger(), "Grabbing thread started"); // load initial configuration for camera into string @@ -503,7 +513,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Error during initial camera configuration: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Error during initial camera configuration: " << ex.what()); } } @@ -512,7 +522,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std if (!ts_host.determineOffset(rcgnodemap_)) { RCLCPP_ERROR_STREAM(this->get_logger(), - "rc_genicam_camera: Cannot determine offset between host and camera clock with maximum tolerance of " + "Cannot determine offset between host and camera clock with maximum tolerance of " << timestamp_tolerance_ << " s"); } @@ -528,7 +538,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std stream[0]->open(); stream[0]->startStreaming(); - RCLCPP_INFO_STREAM(this->get_logger(), "rc_genicam_camera: Start streaming"); + RCLCPP_INFO_STREAM(this->get_logger(), "Start streaming"); // grabbing thread @@ -574,7 +584,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std if (n > 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Dropped images: " << n); + RCLCPP_WARN_STREAM(this->get_logger(), "Dropped images: " << n); } // correct time stamp of image @@ -590,7 +600,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std if (image) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "rc_genicam_camera: Input queue full, dropping image"); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Input queue full, dropping image"); } image.reset(); @@ -614,25 +624,33 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std } else { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Unsupported pixel format"); + RCLCPP_ERROR_STREAM(this->get_logger(), "Unsupported pixel format"); } } + else + { + RCLCPP_WARN_STREAM(this->get_logger(), "Image not present in buffer"); + } } // re-determine offset of host and camera clock if (!ts_host.determineOffset(rcgnodemap_)) { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: Cannot determine offset between host and camera clock with " + RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot determine offset between host and camera clock with " "maximum tolerance of " << timestamp_tolerance_ << " s"); } } else { - RCLCPP_WARN_STREAM(this->get_logger(), "rc_genicam_camera: Incomplete buffer received"); + RCLCPP_WARN_STREAM(this->get_logger(), "Incomplete buffer received"); } } + else + { + RCLCPP_WARN_STREAM(this->get_logger(), "buffer empty"); + } } // stop streaming @@ -642,7 +660,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), ex.what()); std::this_thread::sleep_for(std::chrono::seconds(1)); } @@ -661,15 +679,15 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std } catch (const std::exception& ex) { - RCLCPP_FATAL_STREAM(this->get_logger(), "rc_genicam_camera: " << ex.what()); + RCLCPP_FATAL_STREAM(this->get_logger(), ex.what()); } catch (...) { - RCLCPP_FATAL_STREAM(this->get_logger(),"rc_genicam_camera: Unknown exception"); + RCLCPP_FATAL_STREAM(this->get_logger(),"Unknown exception"); } running_ = false; - RCLCPP_INFO(this->get_logger(), "rc_genicam_camera: Grabbing thread stopped"); + RCLCPP_INFO(this->get_logger(), "Grabbing thread stopped"); } } // namespace rcgccam From efabe527d6411c96a2fb9bf0b5d7d90bfc0a8ceb Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sat, 3 Sep 2022 16:01:48 +0100 Subject: [PATCH 31/32] added config files and install commands --- CMakeLists.txt | 4 ++-- config/basler.config | 12 ++++++++++++ config/flir.config | 8 ++++++++ config/rc_genicam_camera_params.yaml | 26 ++++++++++++++------------ 4 files changed, 36 insertions(+), 14 deletions(-) create mode 100644 config/basler.config create mode 100644 config/flir.config diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fc90a3..9b1482e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,8 +78,8 @@ install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME} ) -# Install launch files. -install(DIRECTORY launch +# Install launch and config files. +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/ ) diff --git a/config/basler.config b/config/basler.config new file mode 100644 index 0000000..9d8aeec --- /dev/null +++ b/config/basler.config @@ -0,0 +1,12 @@ +TriggerMode=Off +AcquisitionMode=Continuous +AcquisitionFrameRateEnable=True +AcquisitionFrameRateAbs=30.00 +ExposureAuto=Continuous +AutoExposureTimeUpperLimitRaw=14000 +AutoExposureTimeLowerLimitRaw=80 +# ExposureTime=14000.00 +# GainAuto=Continuous +AutoGainRawUpperLimit=20.0 +AutoGainRawLowerLimit=1.0 +# Gain=14.00 \ No newline at end of file diff --git a/config/flir.config b/config/flir.config new file mode 100644 index 0000000..245bd1a --- /dev/null +++ b/config/flir.config @@ -0,0 +1,8 @@ +TriggerMode=Off +AcquisitionMode=Continuous +AcquisitionFrameRateEnable=True +AcquisitionFrameRate=30.00 +ExposureAuto=Continuous +# ExposureTime=14997.00 +GainAuto=Continuous +# Gain=14.00 diff --git a/config/rc_genicam_camera_params.yaml b/config/rc_genicam_camera_params.yaml index 1640eb0..9c781a1 100644 --- a/config/rc_genicam_camera_params.yaml +++ b/config/rc_genicam_camera_params.yaml @@ -1,14 +1,16 @@ rc_genicam_camera_node: ros__parameters: - device: - gev_access: - frame_id: - image_prefix: - rotate: - config_file: - calib_file: - calib_id: - host_timestamp: - timestamp_tolerance: - sync_info: - sync_tolderance: \ No newline at end of file + device: "23853944" #BASLER + # device: "18486677" #FLIR1 + # device: "18486668" #FLIR2 + gev_access: "control" #control/exclusive + # frame_id: "cam0" #camera frame ID, default camera[ + # image_prefix: "/tmp/cam0" #to store images + rotate: False #True for rotate 180 + config_file: "install/rc_genicam_camera/share/rc_genicam_camera/config/basler.config" #genicam config file, set wrt ros2 workspace, this is where it is installed check install config dir in cmakelists. or set absolute path + # calib_file: "/path/to/calib" #camera calib file + # calib_id: -1 #0 for left 1 for right, -1 default + # host_timestamp: True #True for using the host time, False default + # timestamp_tolerance: #Maximum acceptable tolerance in seconds between system and camera clock, default 0.01 + # sync_info: #Name of topic that provides CameraInfo messages at the time when image acquisition is triggered via a hardware signal + # sync_tolerance: #Maximum acceptable tolerance in seconds between the CameraInfo timestamps and the image acquisition timestamps \ No newline at end of file From 7cec128a3f4779679505f0c92a1621bea5ad4087 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sat, 3 Sep 2022 16:06:06 +0100 Subject: [PATCH 32/32] ran clang format + added clang command file --- include/genicam_camera_node.h | 8 +-- include/publishers/image_publisher.h | 2 +- include/timestamp_corrector.h | 1 - run_clang_format.sh | 5 ++ src/genicam_camera_node.cc | 68 +++++++++++-------------- src/genicam_camera_node_main.cc | 3 +- src/publishers/camera_info_publisher.cc | 47 ++++++++--------- src/publishers/image_publisher.cc | 51 +++++++++---------- src/timestamp_corrector.cc | 2 +- 9 files changed, 85 insertions(+), 102 deletions(-) create mode 100755 run_clang_format.sh diff --git a/include/genicam_camera_node.h b/include/genicam_camera_node.h index 63847b3..930ed06 100644 --- a/include/genicam_camera_node.h +++ b/include/genicam_camera_node.h @@ -55,7 +55,6 @@ namespace rcgccam { - class GenICamCameraNode : public rclcpp::Node { using rcgc_get_srv = rc_genicam_camera_interfaces::srv::GetGenICamParameter; @@ -67,16 +66,13 @@ class GenICamCameraNode : public rclcpp::Node virtual void onInit(); - bool getGenICamParameter(rcgc_get_srv::Request::SharedPtr req, - rcgc_get_srv::Response::SharedPtr resp); + bool getGenICamParameter(rcgc_get_srv::Request::SharedPtr req, rcgc_get_srv::Response::SharedPtr resp); - bool setGenICamParameter(rcgc_set_srv::Request::SharedPtr req, - rcgc_set_srv::Response::SharedPtr resp); + bool setGenICamParameter(rcgc_set_srv::Request::SharedPtr req, rcgc_set_srv::Response::SharedPtr resp); void syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info); private: - void grab(std::string device, rcg::Device::ACCESS access, std::string config); double timestamp_tolerance_; diff --git a/include/publishers/image_publisher.h b/include/publishers/image_publisher.h index 00784b7..1699090 100644 --- a/include/publishers/image_publisher.h +++ b/include/publishers/image_publisher.h @@ -77,7 +77,7 @@ std::string rosPixelformat(int& bytes_per_pixel, uint64_t pixelformat); Converts a (supported) image in a GenICam buffer into a ROS image. */ sensor_msgs::msg::Image::SharedPtr rosImageFromBuffer(const std::string& frame_id, const rcg::Buffer* buffer, - uint32_t part, bool rotate); + uint32_t part, bool rotate); } // namespace rcgccam diff --git a/include/timestamp_corrector.h b/include/timestamp_corrector.h index c10680f..1ff0516 100644 --- a/include/timestamp_corrector.h +++ b/include/timestamp_corrector.h @@ -40,7 +40,6 @@ namespace rcgccam { - /** This class uses the GenICam command "TimestampLatch" and the parameter "TimestampLatchValue" to determine the offset between the system clock and diff --git a/run_clang_format.sh b/run_clang_format.sh new file mode 100755 index 0000000..a6bb6cd --- /dev/null +++ b/run_clang_format.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +find ./include -regex '.*\.\(h\|hpp\)' -exec clang-format -style=file -i {} \; + +find ./src -regex '.*\.\(cpp\|cc\|cxx\)' -exec clang-format -style=file -i {} \; \ No newline at end of file diff --git a/src/genicam_camera_node.cc b/src/genicam_camera_node.cc index 9a02d52..ea680ad 100644 --- a/src/genicam_camera_node.cc +++ b/src/genicam_camera_node.cc @@ -56,15 +56,14 @@ namespace rcgccam { - GenICamCameraNode::GenICamCameraNode(const std::string& node_name) -: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) + : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) { timestamp_tolerance_ = 0; sync_tolerance_ = 0; rotate_ = false; running_ = false; - + this->declare_parameter("frame_id", ""); this->declare_parameter("device"); this->declare_parameter("gev_access"); @@ -77,7 +76,6 @@ GenICamCameraNode::GenICamCameraNode(const std::string& node_name) this->declare_parameter("sync_tolerance", 0.019); this->declare_parameter("image_prefix", ""); this->declare_parameter("rotate", false); - } GenICamCameraNode::~GenICamCameraNode() @@ -105,7 +103,7 @@ void GenICamCameraNode::onInit() std::string access = "control"; std::string config = ""; std::string calib = ""; - int calib_id=-1; + int calib_id = -1; frame_id_ = this->get_parameter("frame_id").as_string(); @@ -127,7 +125,7 @@ void GenICamCameraNode::onInit() frame_id_ = "camera"; } - int cid=-1; + int cid = -1; cid = this->get_parameter("calib_id").as_int(); if (cid >= 0) @@ -181,10 +179,7 @@ void GenICamCameraNode::onInit() if (sync_info.size() > 0) { sub_sync_info_ptr_ = this->create_subscription( - sync_info, - rclcpp::SensorDataQoS(), - std::bind(&GenICamCameraNode::syncInfo, this, std::placeholders::_1) - ); + sync_info, rclcpp::SensorDataQoS(), std::bind(&GenICamCameraNode::syncInfo, this, std::placeholders::_1)); image_list_.setSize(25); image_list_.setTolerance(static_cast(sync_tolerance_ * 1000000000.0)); @@ -205,7 +200,7 @@ void GenICamCameraNode::onInit() set_param_service_ptr_ = this->create_service( "set_genicam_parameter", std::bind(&GenICamCameraNode::setGenICamParameter, this, std::placeholders::_1, std::placeholders::_2)); - + // initialize publishers caminfo_pub_.init(this->shared_from_this(), calib.c_str(), calib_id); @@ -233,8 +228,7 @@ std::string loadConfig(const std::string& filename) { if (filename.size() > 0) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), - "Reading camera config file: " << filename); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), "Reading camera config file: " << filename); std::ifstream in(filename); std::stringstream buffer; @@ -245,8 +239,7 @@ std::string loadConfig(const std::string& filename) } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), - "Cannot load config: " << filename); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), "Cannot load config: " << filename); } } @@ -262,10 +255,9 @@ std::string loadConfig(const std::string& filename) void applyParameters(const std::shared_ptr& nodemap, const std::string& parameters) { - if(parameters.size() > 0) + if (parameters.size() > 0) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), - "Configuring camera"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rc_genicam_camera_node"), "Configuring camera"); } size_t i = 0; @@ -365,8 +357,7 @@ bool GenICamCameraNode::setGenICamParameter(rcgc_set_srv::Request::SharedPtr req namespace { - -void storeImage(const std::string &prefix, const sensor_msgs::msg::Image::SharedPtr image) +void storeImage(const std::string& prefix, const sensor_msgs::msg::Image::SharedPtr image) { // prepare file name @@ -380,40 +371,38 @@ void storeImage(const std::string &prefix, const sensor_msgs::msg::Image::Shared // store 8 bit images if (image->encoding == sensor_msgs::image_encodings::MONO8 || - image->encoding == sensor_msgs::image_encodings::BAYER_BGGR8 || - image->encoding == sensor_msgs::image_encodings::BAYER_GBRG8 || - image->encoding == sensor_msgs::image_encodings::BAYER_GRBG8 || - image->encoding == sensor_msgs::image_encodings::BAYER_RGGB8) + image->encoding == sensor_msgs::image_encodings::BAYER_BGGR8 || + image->encoding == sensor_msgs::image_encodings::BAYER_GBRG8 || + image->encoding == sensor_msgs::image_encodings::BAYER_GRBG8 || + image->encoding == sensor_msgs::image_encodings::BAYER_RGGB8) { size_t width = image->width; size_t height = image->height; uint8_t* p = reinterpret_cast(&image->data[0]); - FILE *out = fopen(name.str().c_str(), "w"); + FILE* out = fopen(name.str().c_str(), "w"); if (out) { fprintf(out, "P5\n%lu %lu\n255\n", width, height); - size_t n = fwrite(p, 1, width*height, out); + size_t n = fwrite(p, 1, width * height, out); - if (n < width*height) + if (n < width * height) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("storeImage"), - "Cannot write to file " << name.str() << " (" - << n << " < " << width*height << ")"); + "Cannot write to file " << name.str() << " (" << n << " < " << width * height << ")"); } fclose(out); } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("storeImage"), - "Cannot create file " << name.str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("storeImage"), "Cannot create file " << name.str()); } } } -} +} // namespace void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info) { @@ -435,7 +424,7 @@ void GenICamCameraNode::syncInfo(sensor_msgs::msg::CameraInfo::SharedPtr info) if (n > 0) { - RCLCPP_WARN_STREAM(this->get_logger()," Dropped unused images: " << n); + RCLCPP_WARN_STREAM(this->get_logger(), " Dropped unused images: " << n); } // correct time stamp of image @@ -522,8 +511,8 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std if (!ts_host.determineOffset(rcgnodemap_)) { RCLCPP_ERROR_STREAM(this->get_logger(), - "Cannot determine offset between host and camera clock with maximum tolerance of " - << timestamp_tolerance_ << " s"); + "Cannot determine offset between host and camera clock with maximum tolerance of " + << timestamp_tolerance_ << " s"); } // start streaming @@ -600,7 +589,8 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std if (image) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Input queue full, dropping image"); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Input queue full, dropping image"); } image.reset(); @@ -638,8 +628,8 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std if (!ts_host.determineOffset(rcgnodemap_)) { RCLCPP_ERROR_STREAM(this->get_logger(), "Cannot determine offset between host and camera clock with " - "maximum tolerance of " - << timestamp_tolerance_ << " s"); + "maximum tolerance of " + << timestamp_tolerance_ << " s"); } } else @@ -683,7 +673,7 @@ void GenICamCameraNode::grab(std::string device, rcg::Device::ACCESS access, std } catch (...) { - RCLCPP_FATAL_STREAM(this->get_logger(),"Unknown exception"); + RCLCPP_FATAL_STREAM(this->get_logger(), "Unknown exception"); } running_ = false; diff --git a/src/genicam_camera_node_main.cc b/src/genicam_camera_node_main.cc index 5d2b5f5..5d20652 100644 --- a/src/genicam_camera_node_main.cc +++ b/src/genicam_camera_node_main.cc @@ -37,8 +37,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto genicam_camera_node = std::make_shared( - "rc_genicam_camera_node"); + auto genicam_camera_node = std::make_shared("rc_genicam_camera_node"); genicam_camera_node->onInit(); diff --git a/src/publishers/camera_info_publisher.cc b/src/publishers/camera_info_publisher.cc index 0033483..bfe5caf 100644 --- a/src/publishers/camera_info_publisher.cc +++ b/src/publishers/camera_info_publisher.cc @@ -46,7 +46,6 @@ namespace rcgccam { namespace { - /* Remove leading and trailing spaces from string. */ @@ -139,12 +138,13 @@ bool loadFile(std::map& data, const char* name) Create a key like camera[.]. */ -inline std::string createKey(const char *name, int id) +inline std::string createKey(const char* name, int id) { std::ostringstream out; out << "camera."; - if (id >= 0) out << id << '.'; + if (id >= 0) + out << id << '.'; out << name; return out.str(); @@ -156,7 +156,7 @@ inline std::string createKey(const char *name, int id) */ template -void getValue(const std::map& data, const std::string &key, T& value, const char* defvalue) +void getValue(const std::map& data, const std::string& key, T& value, const char* defvalue) { std::map::const_iterator it = data.find(key.c_str()); std::string v; @@ -179,8 +179,7 @@ void getValue(const std::map& data, const std::string returned if the key does not exist. */ -bool getMatrix33(const std::map& data, const std::string &key, - double M[3][3]) +bool getMatrix33(const std::map& data, const std::string& key, double M[3][3]) { std::map::const_iterator it = data.find(key.c_str()); std::string v; @@ -234,8 +233,7 @@ bool getMatrix33(const std::map& data, const std::stri returned if the key does not exist. */ -bool getVector3(const std::map& data, const std::string &key, - double A[3]) +bool getVector3(const std::map& data, const std::string& key, double A[3]) { std::map::const_iterator it = data.find(key.c_str()); std::string v; @@ -278,13 +276,13 @@ bool getVector3(const std::map& data, const std::strin inline void mulMatrix33Matrix33(double ret[3][3], double A[3][3], double B[3][3]) { - for (int k=0; k<3; k++) + for (int k = 0; k < 3; k++) { - for (int i=0; i<3; i++) + for (int i = 0; i < 3; i++) { ret[k][i] = 0; - for (int j=0; j<3; j++) + for (int j = 0; j < 3; j++) { ret[k][i] += A[k][j] * B[j][i]; } @@ -294,10 +292,10 @@ inline void mulMatrix33Matrix33(double ret[3][3], double A[3][3], double B[3][3] inline void mulMatrix33Vector3(double ret[3], double M[3][3], double V[3]) { - for (int k=0; k<3; k++) + for (int k = 0; k < 3; k++) { ret[k] = 0; - for (int i=0; i<3; i++) + for (int i = 0; i < 3; i++) { ret[k] += M[k][i] * V[i]; } @@ -345,10 +343,7 @@ void CameraInfoPublisher::init(rclcpp::Node::SharedPtr node, const char* calib_f { // advertise topic - pub_ptr_ = node->create_publisher( - "camera_info", - rclcpp::SensorDataQoS() - ); + pub_ptr_ = node->create_publisher("camera_info", rclcpp::SensorDataQoS()); // check id @@ -435,7 +430,7 @@ void CameraInfoPublisher::init(rclcpp::Node::SharedPtr node, const char* calib_f double A0[3][3], A1[3][3]; if (getMatrix33(data, createKey("A", 0), A0) && getMatrix33(data, createKey("A", 1), A1)) { - f = (A0[0][0] + A0[1][1] + A1[0][0] + A1[1][1])/4; + f = (A0[0][0] + A0[1][1] + A1[0][0] + A1[1][1]) / 4; } else { @@ -446,7 +441,7 @@ void CameraInfoPublisher::init(rclcpp::Node::SharedPtr node, const char* calib_f } else { - f = (A[0][0] + A[1][1])/2; + f = (A[0][0] + A[1][1]) / 2; } } @@ -490,12 +485,12 @@ void CameraInfoPublisher::init(rclcpp::Node::SharedPtr node, const char* calib_f double Rrect[3][3]; - Rrect[0][0] = T[0]/t; - Rrect[1][0] = T[1]/t; - Rrect[2][0] = T[2]/t; + Rrect[0][0] = T[0] / t; + Rrect[1][0] = T[1] / t; + Rrect[2][0] = T[2] / t; - Rrect[0][1] = -T[1]/l; - Rrect[1][1] = T[0]/l; + Rrect[0][1] = -T[1] / l; + Rrect[1][1] = T[0] / l; Rrect[2][1] = 0; Rrect[0][2] = Rrect[1][0] * Rrect[2][1] - Rrect[2][0] * Rrect[1][1]; @@ -529,11 +524,11 @@ void CameraInfoPublisher::init(rclcpp::Node::SharedPtr node, const char* calib_f info_.p[0] = f; info_.p[1] = 0; - info_.p[2] = info_.width/2.0; + info_.p[2] = info_.width / 2.0; info_.p[3] = 0; info_.p[4] = 0; info_.p[5] = f; - info_.p[6] = info_.height/2.0; + info_.p[6] = info_.height / 2.0; info_.p[7] = 0; info_.p[8] = 0; info_.p[9] = 0; diff --git a/src/publishers/image_publisher.cc b/src/publishers/image_publisher.cc index dc080db..1c609bf 100644 --- a/src/publishers/image_publisher.cc +++ b/src/publishers/image_publisher.cc @@ -44,9 +44,9 @@ namespace rcgccam { - ImagePublisher::ImagePublisher() -{ } +{ +} void ImagePublisher::init(image_transport::ImageTransport& it) { @@ -168,34 +168,33 @@ std::string rosPixelformat(int& bytes_per_pixel, uint64_t pixelformat) namespace { - /* Copies nmemb elements in inverse order. Each element consists of m bytes. The byte order within each element is not inversed. */ -void copyInverse(uint8_t *tp, const uint8_t *sp, size_t nmemb, size_t m) +void copyInverse(uint8_t* tp, const uint8_t* sp, size_t nmemb, size_t m) { if (m == 1) { // optimization of special case of inversing bytewise tp += nmemb; - size_t i=0; + size_t i = 0; -#if defined (__SSSE3__) - __m128i inv_index=_mm_set_epi8(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15); +#if defined(__SSSE3__) + __m128i inv_index = _mm_set_epi8(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15); - while (i+16 <= nmemb) + while (i + 16 <= nmemb) { - tp-=16; + tp -= 16; - __m128i v=_mm_loadu_si128(reinterpret_cast(sp)); - v=_mm_shuffle_epi8(v, inv_index); - _mm_storeu_si128(reinterpret_cast<__m128i *>(tp), v); + __m128i v = _mm_loadu_si128(reinterpret_cast(sp)); + v = _mm_shuffle_epi8(v, inv_index); + _mm_storeu_si128(reinterpret_cast<__m128i*>(tp), v); - sp+=16; - i+=16; + sp += 16; + i += 16; } #endif @@ -210,13 +209,13 @@ void copyInverse(uint8_t *tp, const uint8_t *sp, size_t nmemb, size_t m) { // general case, i.e. m > 1 - tp += m*nmemb; + tp += m * nmemb; - for (size_t i=0; i 0) @@ -281,7 +280,7 @@ sensor_msgs::msg::Image::SharedPtr rosImageFromBuffer(const std::string& frame_i { if (rotate) { - pt += im->step*im->height; + pt += im->step * im->height; for (uint32_t k = 0; k < im->height; k++) { pt -= im->step; @@ -295,7 +294,7 @@ sensor_msgs::msg::Image::SharedPtr rosImageFromBuffer(const std::string& frame_i { for (uint32_t k = 0; k < im->height; k++) { - memcpy(pt, ps, im->step*sizeof(uint8_t)); + memcpy(pt, ps, im->step * sizeof(uint8_t)); pt += im->step; ps += pstep; @@ -306,11 +305,11 @@ sensor_msgs::msg::Image::SharedPtr rosImageFromBuffer(const std::string& frame_i { if (rotate) { - copyInverse(pt, ps, im->height*im->width, bytes_per_pixel); + copyInverse(pt, ps, im->height * im->width, bytes_per_pixel); } else { - memcpy(pt, ps, im->height*im->step*sizeof(uint8_t)); + memcpy(pt, ps, im->height * im->step * sizeof(uint8_t)); } } } diff --git a/src/timestamp_corrector.cc b/src/timestamp_corrector.cc index da720bf..a0aab49 100644 --- a/src/timestamp_corrector.cc +++ b/src/timestamp_corrector.cc @@ -118,7 +118,7 @@ bool TimestampCorrector::determineOffset(const std::shared_ptr> 1) - ts;