diff --git a/CMakeLists.txt b/CMakeLists.txt index 5febf7da..8ef5a6f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -142,6 +142,7 @@ if(rclcpp_FOUND) find_package(sensor_msgs REQUIRED) find_package(rslidar_msg REQUIRED) find_package(std_msgs REQUIRED) + find_package(rclcpp_components REQUIRED) else(rclcpp_FOUND) message(=============================================================) @@ -187,6 +188,20 @@ endif(roscpp_FOUND) #Ros2# if(rclcpp_FOUND) + add_library(rslidar_sdk_component SHARED + node/rslidar_sdk_node_component.cpp + src/manager/node_manager.cpp) + + target_link_libraries(rslidar_sdk_component + ${YAML_CPP_LIBRARIES} + ${rs_driver_LIBRARIES}) + # Component dependencies + ament_target_dependencies(rslidar_sdk_component + rclcpp + rclcpp_components + std_msgs + sensor_msgs + rslidar_msg) ament_target_dependencies(rslidar_sdk_node rclcpp @@ -196,7 +211,12 @@ if(rclcpp_FOUND) install(TARGETS rslidar_sdk_node - DESTINATION lib/${PROJECT_NAME}) + rslidar_sdk_component + RUNTIME DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib) + + # Register component with ament index + rclcpp_components_register_nodes(rslidar_sdk_component "robosense::lidar::RSLidarSDKComponent") install(DIRECTORY launch diff --git a/launch/start_component.launch.py b/launch/start_component.launch.py new file mode 100644 index 00000000..36a814e2 --- /dev/null +++ b/launch/start_component.launch.py @@ -0,0 +1,28 @@ +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + container = ComposableNodeContainer( + name='rslidar_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='rslidar_sdk', + plugin='robosense::lidar::RSLidarSDKComponent', + name='rslidar_node', + parameters=[{ + 'config_path': '', + }] + ), + ], + output='screen', + ) + + # Create the launch description and populate + ld = LaunchDescription() + ld.add_action(container) + return ld diff --git a/node/rslidar_sdk_node_component.cpp b/node/rslidar_sdk_node_component.cpp new file mode 100644 index 00000000..4f434db2 --- /dev/null +++ b/node/rslidar_sdk_node_component.cpp @@ -0,0 +1,106 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other 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 OWNER 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 "manager/node_manager.hpp" +#include +#include +#include +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ + +class RSLidarSDKComponent : public rclcpp::Node +{ +public: + explicit RSLidarSDKComponent(const rclcpp::NodeOptions & options) + : Node("rslidar_sdk_node_component", options) + { + // Print version information + RCLCPP_INFO(this->get_logger(), "********************************************************"); + RCLCPP_INFO(this->get_logger(), "********** **********"); + RCLCPP_INFO(this->get_logger(), "********** RSLidar_SDK Version: v%d.%d.%d **********", + RSLIDAR_VERSION_MAJOR, RSLIDAR_VERSION_MINOR, RSLIDAR_VERSION_PATCH); + RCLCPP_INFO(this->get_logger(), "********** **********"); + RCLCPP_INFO(this->get_logger(), "********************************************************"); + + // Set up config path + std::string config_path; + +#ifdef RUN_IN_ROS_WORKSPACE + config_path = ament_index_cpp::get_package_share_directory("rslidar_sdk"); +#else + config_path = (std::string)PROJECT_PATH; +#endif + + config_path += "/config/config.yaml"; + + std::string path = this->declare_parameter("config_path", ""); + if (!path.empty()) + { + config_path = path; + } + + YAML::Node config; + try + { + config = YAML::LoadFile(config_path); + RCLCPP_INFO(this->get_logger(), "--------------------------------------------------------"); + RCLCPP_INFO(this->get_logger(), "Config loaded from PATH:"); + RCLCPP_INFO(this->get_logger(), "%s", config_path.c_str()); + RCLCPP_INFO(this->get_logger(), "--------------------------------------------------------"); + } + catch (...) + { + RCLCPP_ERROR(this->get_logger(), "The format of config file %s is wrong. Please check (e.g. indentation).", + config_path.c_str()); + throw std::runtime_error("Failed to load config file"); + } + + node_manager_ = std::make_shared(); + node_manager_->init(config); + node_manager_->start(); + } + +private: + std::shared_ptr node_manager_; +}; + +} // namespace lidar +} // namespace robosense + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(robosense::lidar::RSLidarSDKComponent) diff --git a/package.xml b/package.xml index 02ad159d..3d96d4a9 100644 --- a/package.xml +++ b/package.xml @@ -15,6 +15,7 @@ roscpp roslib rslidar_msg + rclcpp_components sensor_msgs std_msgs yaml-cpp