diff --git a/pointcloud_crop_box/CMakeLists.txt b/pointcloud_crop_box/CMakeLists.txt index 950c6ee..e263f0a 100644 --- a/pointcloud_crop_box/CMakeLists.txt +++ b/pointcloud_crop_box/CMakeLists.txt @@ -12,6 +12,7 @@ set(PACKAGE_DEPENDENCIES pcl_conversions pcl_ros rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs @@ -30,18 +31,29 @@ include_directories(include ${PCL_INCLUDE_DIRS}) generate_parameter_library(pointcloud_crop_box_params src/pointcloud_crop_box_params.yaml) -add_executable(pointcloud_crop_box_node src/main.cpp - src/pointcloud_crop_box_node.cpp) +add_library(pointcloud_crop_box_node_component SHARED + src/pointcloud_crop_box_node.cpp) -ament_target_dependencies(pointcloud_crop_box_node ${PACKAGE_DEPENDENCIES}) +ament_target_dependencies(pointcloud_crop_box_node_component + ${PACKAGE_DEPENDENCIES}) -target_link_libraries(pointcloud_crop_box_node ${PCL_LIBRARIES} +target_link_libraries(pointcloud_crop_box_node_component ${PCL_LIBRARIES} pointcloud_crop_box_params) target_include_directories( - pointcloud_crop_box_node + pointcloud_crop_box_node_component PUBLIC $) -install(TARGETS pointcloud_crop_box_node DESTINATION lib/${PROJECT_NAME}) +rclcpp_components_register_node( + pointcloud_crop_box_node_component PLUGIN + "pointcloud_crop_box::PointCloudCropBoxNode" EXECUTABLE + pointcloud_crop_box_node) + +install( + TARGETS pointcloud_crop_box_node_component pointcloud_crop_box_params + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) diff --git a/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp b/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp index 6aaef47..897ac32 100644 --- a/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp +++ b/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp @@ -35,13 +35,16 @@ #include "pointcloud_crop_box/pointcloud_crop_box_params.hpp" -class PointcloudCropBoxNode : public rclcpp::Node +namespace pointcloud_crop_box +{ + +class PointCloudCropBoxNode : public rclcpp::Node { public: - PointcloudCropBoxNode(); + PointCloudCropBoxNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: - void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void PointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); void LaserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg); geometry_msgs::msg::TransformStamped GetTransform(const std::string & source_frame); @@ -71,4 +74,6 @@ class PointcloudCropBoxNode : public rclcpp::Node laser_geometry::LaserProjection projector_; }; +} // namespace pointcloud_crop_box + #endif // POINTCLOUD_CROP_BOX_POINTCLOUD_CROP_BOX_POINTCLOUD_CROP_BOX_NODE_HPP_ diff --git a/pointcloud_crop_box/package.xml b/pointcloud_crop_box/package.xml index 6351c68..c829150 100644 --- a/pointcloud_crop_box/package.xml +++ b/pointcloud_crop_box/package.xml @@ -3,7 +3,7 @@ pointcloud_crop_box 0.1.0 - 3D box filter for Pointcloud2 + 3D box filter for PointCloud2 Husarion Apache License 2.0 @@ -12,6 +12,7 @@ https://github.com/husarion/husarion_ugv_ros/issues Jakub Delicat + Dawid Kmak ament_cmake @@ -20,6 +21,7 @@ pcl_conversions pcl_ros rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs @@ -27,6 +29,8 @@ tf2_sensor_msgs vision_msgs + vision_msgs_rviz_plugins + ament_cmake diff --git a/pointcloud_crop_box/src/main.cpp b/pointcloud_crop_box/src/main.cpp deleted file mode 100644 index dbfb805..0000000 --- a/pointcloud_crop_box/src/main.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2025 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include - -#include - -#include "pointcloud_crop_box/pointcloud_crop_box_node.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto pointcloud_crop_box = std::make_shared(); - - try { - rclcpp::spin(pointcloud_crop_box); - } catch (const std::runtime_error & e) { - std::cerr << "[" << pointcloud_crop_box->get_name() << "] Caught exception: " << e.what() - << std::endl; - } - - std::cout << "[" << pointcloud_crop_box->get_name() << "] Shutting down" << std::endl; - rclcpp::shutdown(); - return 0; -} diff --git a/pointcloud_crop_box/src/pointcloud_crop_box_node.cpp b/pointcloud_crop_box/src/pointcloud_crop_box_node.cpp index cfb31a8..39070fb 100644 --- a/pointcloud_crop_box/src/pointcloud_crop_box_node.cpp +++ b/pointcloud_crop_box/src/pointcloud_crop_box_node.cpp @@ -24,8 +24,13 @@ #include #include -PointcloudCropBoxNode::PointcloudCropBoxNode() -: rclcpp::Node("pointcloud_crop_box"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +namespace pointcloud_crop_box +{ + +PointCloudCropBoxNode::PointCloudCropBoxNode(const rclcpp::NodeOptions & options) +: rclcpp::Node("pointcloud_crop_box", options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) { param_listener_ = std::make_shared(get_node_parameters_interface()); @@ -36,11 +41,11 @@ PointcloudCropBoxNode::PointcloudCropBoxNode() if (params_.message_type == "pointcloud") { pc2_sub_ = this->create_subscription( params_.input_topic, 10, - std::bind(&PointcloudCropBoxNode::PointcloudCallback, this, std::placeholders::_1)); + std::bind(&PointCloudCropBoxNode::PointCloudCallback, this, std::placeholders::_1)); } else { ls_sub_ = this->create_subscription( params_.input_topic, 10, - std::bind(&PointcloudCropBoxNode::LaserScanCallback, this, std::placeholders::_1)); + std::bind(&PointCloudCropBoxNode::LaserScanCallback, this, std::placeholders::_1)); } if (params_.visualize_bounding_box) { @@ -54,7 +59,7 @@ PointcloudCropBoxNode::PointcloudCropBoxNode() RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void PointcloudCropBoxNode::PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +void PointCloudCropBoxNode::PointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); @@ -96,15 +101,15 @@ void PointcloudCropBoxNode::PointcloudCallback(const sensor_msgs::msg::PointClou } } -void PointcloudCropBoxNode::LaserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) +void PointCloudCropBoxNode::LaserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { auto cloud_msg = std::make_shared(); projector_.projectLaser(*msg, *cloud_msg); - PointcloudCallback(cloud_msg); + PointCloudCallback(cloud_msg); } -geometry_msgs::msg::TransformStamped PointcloudCropBoxNode::GetTransform( +geometry_msgs::msg::TransformStamped PointCloudCropBoxNode::GetTransform( const std::string & source_frame) { geometry_msgs::msg::TransformStamped transform_stamped; @@ -121,7 +126,7 @@ geometry_msgs::msg::TransformStamped PointcloudCropBoxNode::GetTransform( return transform_stamped; } -pcl::PointCloud::Ptr PointcloudCropBoxNode::TransformCloud( +pcl::PointCloud::Ptr PointCloudCropBoxNode::TransformCloud( const pcl::PointCloud::Ptr & cloud, const geometry_msgs::msg::TransformStamped & transform_stamped) { @@ -137,7 +142,7 @@ pcl::PointCloud::Ptr PointcloudCropBoxNode::TransformCloud( return transformed_cloud; } -pcl::PointCloud::Ptr PointcloudCropBoxNode::Crop( +pcl::PointCloud::Ptr PointCloudCropBoxNode::Crop( const pcl::PointCloud::Ptr & cloud) { pcl::CropBox crop_box_filter; @@ -151,7 +156,7 @@ pcl::PointCloud::Ptr PointcloudCropBoxNode::Crop( return filtered_cloud; } -geometry_msgs::msg::TransformStamped PointcloudCropBoxNode::InverseTransform( +geometry_msgs::msg::TransformStamped PointCloudCropBoxNode::InverseTransform( const geometry_msgs::msg::TransformStamped & transform_stamped) { tf2::Transform tf2_transform; @@ -180,7 +185,7 @@ geometry_msgs::msg::TransformStamped PointcloudCropBoxNode::InverseTransform( return inverse_transform_stamped; } -vision_msgs::msg::BoundingBox3D PointcloudCropBoxNode::CreateBoundingBox() +vision_msgs::msg::BoundingBox3D PointCloudCropBoxNode::CreateBoundingBox() { vision_msgs::msg::BoundingBox3D bbox; @@ -194,3 +199,8 @@ vision_msgs::msg::BoundingBox3D PointcloudCropBoxNode::CreateBoundingBox() return bbox; } + +} // namespace pointcloud_crop_box + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_crop_box::PointCloudCropBoxNode)