diff --git a/astra_camera/CMakeLists.txt b/astra_camera/CMakeLists.txt index 9ce43e8..36ccede 100644 --- a/astra_camera/CMakeLists.txt +++ b/astra_camera/CMakeLists.txt @@ -213,6 +213,14 @@ target_link_libraries(clean_shm_node ${PROJECT_NAME} ) +target_compile_definitions(${PROJECT_NAME} PRIVATE "COMPOSITION_BUILDING_DLL") + +rclcpp_components_register_nodes(${PROJECT_NAME} + "${PROJECT_NAME}::PointCloudXyzrgbNode" + "${PROJECT_NAME}::PointCloudXyzNode" + "${PROJECT_NAME}::OBCameraNodeFactory") + + install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/astra_camera/include/astra_camera/ob_camera_node.h b/astra_camera/include/astra_camera/ob_camera_node.h index a424d6d..a0cbd70 100644 --- a/astra_camera/include/astra_camera/ob_camera_node.h +++ b/astra_camera/include/astra_camera/ob_camera_node.h @@ -45,7 +45,6 @@ #include "uvc_camera_driver.h" #include "dynamic_params.h" #include "types.h" -#include "point_cloud_proc/point_cloud_proc.h" #include "magic_enum/magic_enum.hpp" @@ -219,8 +218,7 @@ class OBCameraNode { std::map stream_video_mode_; std::map> supported_video_modes_; std::map unit_step_size_; - std::map::SharedPtr> - image_publishers_; + std::map image_publishers_; std::map::SharedPtr> camera_info_publishers_; @@ -260,8 +258,6 @@ class OBCameraNode { ImageROI depth_roi_; int depth_scale_ = 1; std::string point_cloud_qos_; - std::unique_ptr point_cloud_processor_ = nullptr; - std::unique_ptr colored_point_cloud_processor_ = nullptr; bool enable_point_cloud_ = true; bool enable_colored_point_cloud_ = false; std::recursive_mutex device_lock_; diff --git a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_proc.h b/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_proc.h deleted file mode 100644 index acb1dce..0000000 --- a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_proc.h +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once - -#include "point_cloud_xyz.h" -#include "point_cloud_xyzrgb.h" diff --git a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h b/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h index 9ca9858..f0b5d68 100644 --- a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h +++ b/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h @@ -45,9 +45,9 @@ namespace astra_camera { namespace enc = sensor_msgs::image_encodings; -class PointCloudXyzNode { +class PointCloudXyzNode : public rclcpp::Node { public: - explicit PointCloudXyzNode(rclcpp::Node* const node, std::shared_ptr parameters); + explicit PointCloudXyzNode(const rclcpp::NodeOptions& options); template static void convertDepth(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, @@ -58,7 +58,6 @@ class PointCloudXyzNode { using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; using CameraInfo = sensor_msgs::msg::CameraInfo; - rclcpp::Node* const node_; std::shared_ptr parameters_; rclcpp::Logger logger_ = rclcpp::get_logger("PointCloudXyzNode"); // Subscriptions diff --git a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyzrgb.h b/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyzrgb.h index 911e67f..0b6ec63 100644 --- a/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyzrgb.h +++ b/astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyzrgb.h @@ -59,9 +59,9 @@ namespace astra_camera { namespace enc = sensor_msgs::image_encodings; -class PointCloudXyzrgbNode { +class PointCloudXyzrgbNode : public rclcpp::Node{ public: - explicit PointCloudXyzrgbNode(rclcpp::Node* node, std::shared_ptr parameters); + explicit PointCloudXyzrgbNode(const rclcpp::NodeOptions& options); void convertRgb(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg, sensor_msgs::msg::PointCloud2::SharedPtr& cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step); @@ -70,7 +70,6 @@ class PointCloudXyzrgbNode { using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; using CameraInfo = sensor_msgs::msg::CameraInfo; - rclcpp::Node* const node_; std::shared_ptr parameters_; rmw_qos_profile_t point_cloud_qos_profile_; rmw_qos_profile_t color_qos_profile_, depth_qos_profile_, info_qos_profile_; diff --git a/astra_camera/include/astra_camera/uvc_camera_driver.h b/astra_camera/include/astra_camera/uvc_camera_driver.h index 859b7c9..60b8e2a 100644 --- a/astra_camera/include/astra_camera/uvc_camera_driver.h +++ b/astra_camera/include/astra_camera/uvc_camera_driver.h @@ -126,7 +126,7 @@ class UVCCameraDriver { std::shared_ptr parameters_; UVCCameraConfig config_; ImageROI roi_; - std::string camera_name_ = "camera"; + std::string camera_name_ = "head_front_camera"; std::string color_info_url_; uvc_context_t* ctx_ = nullptr; uvc_device_t* device_ = nullptr; @@ -151,7 +151,7 @@ class UVCCameraDriver { rclcpp::Service::SharedPtr set_uvc_mirror_srv_; rclcpp::Service::SharedPtr toggle_uvc_camera_srv_; - rclcpp::Publisher::SharedPtr image_publisher_; + image_transport::Publisher image_publisher_; rclcpp::Publisher::SharedPtr camera_info_publisher_; sensor_msgs::msg::CameraInfo camera_info_; std::unique_ptr camera_info_manager_ = nullptr; diff --git a/astra_camera/src/ob_camera_node.cpp b/astra_camera/src/ob_camera_node.cpp index bafc36e..314406c 100644 --- a/astra_camera/src/ob_camera_node.cpp +++ b/astra_camera/src/ob_camera_node.cpp @@ -23,12 +23,6 @@ void OBCameraNode::init() { startStreams(); run_streaming_poller_ = true; poll_stream_thread_ = std::make_shared([this]() { pollFrame(); }); - if (enable_point_cloud_) { - point_cloud_processor_ = std::make_unique(node_, parameters_); - } - if (enable_colored_point_cloud_) { - colored_point_cloud_processor_ = std::make_unique(node_, parameters_); - } is_initialized_ = true; } @@ -383,9 +377,7 @@ void OBCameraNode::setupPublishers() { std::string topic = name + "/image_raw"; auto image_qos = image_qos_[stream_index]; auto image_qos_profile = getRMWQosProfileFromString(image_qos); - image_publishers_[stream_index] = node_->create_publisher( - topic, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile)); + image_publishers_[stream_index] = image_transport::create_publisher(node_, topic, image_qos_profile); topic = name + "/camera_info"; auto camera_info_qos = camera_info_qos_[stream_index]; auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos); @@ -531,7 +523,7 @@ void OBCameraNode::onNewFrameCallback(const openni::VideoFrameRef& frame, image_msg->height = stream_index == DEPTH ? height * depth_scale_ : height; image_msg->step = image_msg->width * unit_step_size_[stream_index]; image_msg->is_bigendian = false; - image_publisher->publish(*image_msg); + image_publisher.publish(image_msg); sensor_msgs::msg::CameraInfo camera_info; if (stream_index == DEPTH) { camera_info = getDepthCameraInfo(); diff --git a/astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp b/astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp index 0c5a599..005c3c5 100644 --- a/astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp +++ b/astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp @@ -39,10 +39,10 @@ namespace astra_camera { -PointCloudXyzNode::PointCloudXyzNode(rclcpp::Node *const node, - std::shared_ptr parameters) - : node_(node), parameters_(std::move(parameters)) { +PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions& options) + : Node("PointCloudXyzNode", options){ // Read parameters + parameters_ = std::make_shared(this); setAndGetNodeParameter(parameters_, queue_size_, "queue_size", 5); std::string point_cloud_qos; std::string depth_qos; @@ -59,7 +59,7 @@ PointCloudXyzNode::PointCloudXyzNode(rclcpp::Node *const node, std::scoped_lock lock(connect_mutex_); // TODO(ros2) Implement when SubscriberStatusCallback is available point_cloud_qos_profile_ = getRMWQosProfileFromString(point_cloud_qos); - pub_point_cloud_ = node_->create_publisher( + pub_point_cloud_ = this->create_publisher( "depth/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile_), point_cloud_qos_profile_)); } @@ -114,7 +114,7 @@ void PointCloudXyzNode::connectCb() { custom_qos.depth = queue_size_; sub_depth_ = image_transport::create_camera_subscription( - node_, "depth/image_raw", + this, "depth/image_raw", [this](const sensor_msgs::msg::Image::ConstSharedPtr &msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &info) { depthCb(msg, info); }, "raw", custom_qos); @@ -150,3 +150,10 @@ void PointCloudXyzNode::depthCb(const Image::ConstSharedPtr &depth_msg, } } // namespace astra_camera + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(astra_camera::PointCloudXyzNode) + + diff --git a/astra_camera/src/point_cloud_proc/point_cloud_xyzrgb.cpp b/astra_camera/src/point_cloud_proc/point_cloud_xyzrgb.cpp index 7609fa4..6a893e5 100644 --- a/astra_camera/src/point_cloud_proc/point_cloud_xyzrgb.cpp +++ b/astra_camera/src/point_cloud_proc/point_cloud_xyzrgb.cpp @@ -43,16 +43,13 @@ #include #include -#include -#include -#include #include "astra_camera/point_cloud_proc/point_cloud_xyzrgb.h" namespace astra_camera { -PointCloudXyzrgbNode::PointCloudXyzrgbNode(rclcpp::Node* const node, - std::shared_ptr parameters) - : node_(node), parameters_(std::move(parameters)) { +PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions& options) + : Node("PointCloudXyzrgbNode", options) { // Read parameters + parameters_ = std::make_shared(this); int queue_size = 5; bool use_exact_sync = false; setAndGetNodeParameter(parameters_, queue_size, "queue_size", 5); @@ -90,7 +87,7 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(rclcpp::Node* const node, // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ std::lock_guard lock(connect_mutex_); // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available - pub_point_cloud_ = node_->create_publisher( + pub_point_cloud_ = this->create_publisher( "depth/color/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile_), point_cloud_qos_profile_)); @@ -121,15 +118,15 @@ void PointCloudXyzrgbNode::connectCb() { // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it // parameter for depth_image_transport hint std::string depth_image_transport_param = "depth_image_transport"; - image_transport::TransportHints depth_hints(node_, "raw", depth_image_transport_param); + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); // depth image can use different transport.(e.g. compressedDepth) - sub_depth_.subscribe(node_, "depth/image_raw", depth_hints.getTransport(), depth_qos_profile_); + sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport(), depth_qos_profile_); // rgb uses normal ros transport hints. - image_transport::TransportHints hints(node_, "raw"); - sub_rgb_.subscribe(node_, "color/image_raw", hints.getTransport(), color_qos_profile_); - sub_info_.subscribe(node_, "color/camera_info", info_qos_profile_); + image_transport::TransportHints hints(this, "raw"); + sub_rgb_.subscribe(this, "color/image_raw", hints.getTransport(), color_qos_profile_); + sub_info_.subscribe(this, "color/camera_info", info_qos_profile_); } void PointCloudXyzrgbNode::imageCb(const Image::ConstSharedPtr& depth_msg, @@ -186,7 +183,7 @@ void PointCloudXyzrgbNode::imageCb(const Image::ConstSharedPtr& depth_msg, rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg(); } - RCLCPP_ERROR_THROTTLE(logger_, *(node_->get_clock()), 50000, + RCLCPP_ERROR_THROTTLE(logger_, *(this->get_clock()), 50000, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)", depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height); return; @@ -260,3 +257,8 @@ void PointCloudXyzrgbNode::imageCb(const Image::ConstSharedPtr& depth_msg, } } // namespace astra_camera + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(astra_camera::PointCloudXyzrgbNode) diff --git a/astra_camera/src/uvc_camera_driver.cpp b/astra_camera/src/uvc_camera_driver.cpp index fa15e50..a590d86 100644 --- a/astra_camera/src/uvc_camera_driver.cpp +++ b/astra_camera/src/uvc_camera_driver.cpp @@ -87,9 +87,7 @@ UVCCameraDriver::UVCCameraDriver(rclcpp::Node* node, std::shared_ptr camera_info_manager_ = std::make_unique( node_, "rgb_camera", color_info_url_); } - image_publisher_ = node_->create_publisher( - "color/image_raw", - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(color_qos_profile_), color_qos_profile_)); + image_publisher_ = image_transport::create_publisher(node_, "color/image_raw", color_qos_profile_); camera_info_publisher_ = node_->create_publisher( "color/camera_info", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(color_info_qos_profile_), color_info_qos_profile_)); @@ -395,7 +393,7 @@ void UVCCameraDriver::frameCallback(uvc_frame_t* frame) { cv_image_ptr->image = cv_img; image = *(cv_image_ptr->toImageMsg()); } - image_publisher_->publish(image); + image_publisher_.publish(image); auto camera_info = getCameraInfo(); camera_info.header.stamp = image.header.stamp; camera_info.header.frame_id = image.header.frame_id;