diff --git a/.gitignore b/.gitignore index 8499473..41c354a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ *.user -.vscode/ \ No newline at end of file +.vscode/ +build +install +log \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 6d35f93..139b01e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,153 +1,85 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ira_laser_tools) -add_compile_options(-std=c++14) - -## 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 laser_geometry roscpp sensor_msgs std_msgs tf dynamic_reconfigure pcl_ros) +# Default to C99 +set(CMAKE_C_STANDARD 99) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(laser_geometry REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pcl_ros REQUIRED) find_package(Eigen3 REQUIRED) - find_package(PCL REQUIRED) +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + geometry_msgs + sensor_msgs + laser_geometry + tf2 + tf2_ros + pcl_ros + Eigen3 + PCL +) + + # bugfix for g++-Warning # ":0:0: warning: missing whitespace after the macro name" remove_definitions(-DDISABLE_LIBUSB-1.0) -generate_dynamic_reconfigure_options(cfg/laserscan_multi_merger.cfg cfg/laserscan_virtualizer.cfg) - - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -####################################### -## Declare ROS messages and services ## -####################################### - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# sensor_msgs# std_msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## 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 -# LIBRARIES laser_merger -# CATKIN_DEPENDS laser_geometry roscpp sensor_msgs std_msgs tf -# DEPENDS system_lib -) - ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations -include_directories(include - ${catkin_INCLUDE_DIRS} - ${EIGEN_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -#add_definitions(${EIGEN_DEFINITIONS}) - -## Declare a cpp library -# add_library(laser_merger -# src/${PROJECT_NAME}/laser_merger.cpp -# ) - -## Declare a cpp executable -#add_executable(laser_merger_node src/laser_merger_node.cpp) -#target_link_libraries(laser_merger_node ${catkin_LIBRARIES}) add_executable(laserscan_multi_merger src/laserscan_multi_merger.cpp) -target_link_libraries(laserscan_multi_merger ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(laserscan_multi_merger ${PROJECT_NAME}_gencfg) +ament_target_dependencies(laserscan_multi_merger ${dependencies}) +target_include_directories(laserscan_multi_merger PUBLIC ${EIGEN_INCLUDE_DIRS}) +target_include_directories(laserscan_multi_merger PUBLIC ${PCL_INCLUDE_DIRS}) +target_link_libraries(laserscan_multi_merger ${PCL_LIBRARIES}) add_executable(laserscan_virtualizer src/laserscan_virtualizer.cpp) -target_link_libraries(laserscan_virtualizer ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(laserscan_virtualizer ${PROJECT_NAME}_gencfg) - -## Specify libraries to link a library or executable target against - +ament_target_dependencies(laserscan_virtualizer ${dependencies}) +target_include_directories(laserscan_virtualizer PUBLIC ${EIGEN_INCLUDE_DIRS}) +target_include_directories(laserscan_virtualizer PUBLIC ${PCL_INCLUDE_DIRS}) +target_link_libraries(laserscan_virtualizer ${PCL_LIBRARIES}) ############# ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - ## Mark executables and/or libraries for installation install(TARGETS laserscan_multi_merger laserscan_virtualizer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - ## Mark other files for installation (e.g. launch and bag files, etc.) install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + DESTINATION share/${PROJECT_NAME}/launch ) -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_laser_merger.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_export_dependencies(${dependencies}) +ament_package() \ No newline at end of file diff --git a/cfg/laserscan_multi_merger.cfg b/cfg/laserscan_multi_merger.cfg deleted file mode 100755 index 5705022..0000000 --- a/cfg/laserscan_multi_merger.cfg +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "laserscan_multi_merger" - -from dynamic_reconfigure.parameter_generator_catkin import * -import math - -gen = ParameterGenerator() - -gen.add("angle_min", double_t, 0, "Minimum angle of the output scan", -2.36, -math.pi, math.pi) -gen.add("angle_max", double_t, 0, "Maximum angle of the output scan", 2.36, -math.pi, math.pi) -gen.add("angle_increment", double_t, 0, "Angle increment of the output scan", 0.0058, 0, math.pi) -gen.add("time_increment", double_t, 0, "Time increment of the output scan", 0.0, 0, 1) -gen.add("scan_time", double_t, 0, "Scan time of the output scan", 0.033333333, 0, 1) -gen.add("range_min", double_t, 0, "Range min of the output scan", 0.45, 0, 50) -gen.add("range_max", double_t, 0, "Range max of the output scan", 25.0, 0, 1000) - -exit(gen.generate(PACKAGE, "laserscan_multi_merger", "laserscan_multi_merger")) diff --git a/cfg/laserscan_virtualizer.cfg b/cfg/laserscan_virtualizer.cfg deleted file mode 100755 index 9610084..0000000 --- a/cfg/laserscan_virtualizer.cfg +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "laserscan_virtualizer" - -from dynamic_reconfigure.parameter_generator_catkin import * -import math - -gen = ParameterGenerator() - -gen.add("angle_min", double_t, 0, "Minimum angle of the output scan", -2.36, -math.pi, math.pi) -gen.add("angle_max", double_t, 0, "Maximum angle of the output scan", 2.36, -math.pi, math.pi) -gen.add("angle_increment", double_t, 0, "Angle increment of the output scan", 0.0058, 0, math.pi) -gen.add("time_increment", double_t, 0, "Time increment of the output scan", 0.0, 0, 1) -gen.add("scan_time", double_t, 0, "Scan time of the output scan", 0.033333333, 0, 1) -gen.add("range_min", double_t, 0, "Range min of the output scan", 0.45, 0, 50) -gen.add("range_max", double_t, 0, "Range max of the output scan", 25.0, 0, 50) - -exit(gen.generate(PACKAGE, "laserscan_virtualizer", "laserscan_virtualizer")) diff --git a/launch/laserscan_multi_merger.launch b/launch/laserscan_multi_merger.launch index e6fad47..d014030 100644 --- a/launch/laserscan_multi_merger.launch +++ b/launch/laserscan_multi_merger.launch @@ -1,16 +1,17 @@ - - - - - - - - - - - + + + + + + + + + + + + diff --git a/launch/laserscan_virtualizer.launch b/launch/laserscan_virtualizer.launch index 4b3b91b..0b232d1 100644 --- a/launch/laserscan_virtualizer.launch +++ b/launch/laserscan_virtualizer.launch @@ -2,8 +2,7 @@ FROM: http://wiki.ros.org/tf#static_transform_publisher <> -Publish a static coordinate transform to tf using an x/y/z offset and yaw/pitch/roll. The period, in milliseconds, specifies how often to send a transform. 100ms (10hz) is a good value. -== OR == +Publish a static coordinate transform to tf using an x/y/z offset and yaw/pitch/roll. The period, in milliseconds, specifies how often to send a transform. 100ms (10hz) is a good value. == OR == <> Publish a static coordinate transform to tf using an x/y/z offset and quaternion. The period, in milliseconds, specifies how often to send a transform. 100ms (10hz) is a good value. @@ -13,13 +12,23 @@ Publish a static coordinate transform to tf using an x/y/z offset and quaternion - - + + - - - - - + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index f4f7098..e938ebb 100644 --- a/package.xml +++ b/package.xml @@ -1,34 +1,38 @@ - + ira_laser_tools - 1.0.7 + 2.0.7 The ira_laser_tools package. These nodes are meant to provide some utils for lasers, like listen to different laser scan sources and merge them in a single scan or generate virtual laser scans from a pointcloud. http://www.ros.org/wiki/ira_laser_tools Augusto Pietro Fabio - + BSD - catkin - sensor_msgs - libpcl-all-dev - pcl_ros - std_msgs - tf - laser_geometry - roscpp - sensor_msgs - libpcl-all-dev - pcl_ros - std_msgs - tf - laser_geometry - roscpp + ament_cmake + sensor_msgs + libpcl-all-dev + pcl_ros + geometry_msgs + std_msgs + tf2 + tf2_ros + laser_geometry + rclcpp + rclcpp_action + rclcpp_lifecycle - libvtk-qt - libvtk-qt - + libvtk-qt + + rosidl_default_runtime + eigen3 + pcl + + + ament_cmake + + diff --git a/src/laserscan_multi_merger.cpp b/src/laserscan_multi_merger.cpp index c5428f6..d8fb5b4 100644 --- a/src/laserscan_multi_merger.cpp +++ b/src/laserscan_multi_merger.cpp @@ -1,93 +1,175 @@ -#include #include -#include -#include -#include +#include +#include #include #include #include -#include -#include -#include -#include "sensor_msgs/LaserScan.h" -#include "pcl_ros/point_cloud.h" -#include -#include -#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "pcl_ros/transforms.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" using namespace std; using namespace pcl; -using namespace laserscan_multi_merger; -class LaserscanMerger +using std::placeholders::_1; + +class LaserscanMerger : public rclcpp::Node { - public: - LaserscanMerger(); - void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan, std::string topic); - void pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud); - void reconfigureCallback(laserscan_multi_mergerConfig &config, uint32_t level); - - private: - ros::NodeHandle node_; - laser_geometry::LaserProjection projector_; - tf::TransformListener tfListener_; - - ros::Publisher point_cloud_publisher_; - ros::Publisher laser_scan_publisher_; - vector scan_subscribers; - vector clouds_modified; - - vector clouds; - vector input_topics; - - void laserscan_topic_parser(); - - double angle_min; - double angle_max; - double angle_increment; - double time_increment; - double scan_time; - double range_min; - double range_max; - - string destination_frame; - string cloud_destination_topic; - string scan_destination_topic; - string laserscan_topics; +public: + LaserscanMerger(); + void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan, std::string topic); + void pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud); + rcl_interfaces::msg::SetParametersResult reconfigureCallback(const std::vector ¶meters); + +private: + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + laser_geometry::LaserProjection projector_; + std::unique_ptr tf_buffer_; + std::shared_ptr tfListener_; + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + rclcpp::Publisher::SharedPtr point_cloud_publisher_; + rclcpp::Publisher::SharedPtr laser_scan_publisher_; + std::vector::SharedPtr> scan_subscribers; + std::vector clouds_modified; + + std::vector clouds; + std::vector input_topics; + + void laserscan_topic_parser(); + + double angle_min; + double angle_max; + double angle_increment; + double time_increment; + double scan_time; + double range_min; + double range_max; + + string destination_frame; + string cloud_destination_topic; + string scan_destination_topic; + string laserscan_topics; }; -void LaserscanMerger::reconfigureCallback(laserscan_multi_mergerConfig &config, uint32_t level) +LaserscanMerger::LaserscanMerger() : Node("laserscan_multi_merger") { - this->angle_min = config.angle_min; - this->angle_max = config.angle_max; - this->angle_increment = config.angle_increment; - this->time_increment = config.time_increment; - this->scan_time = config.scan_time; - this->range_min = config.range_min; - this->range_max = config.range_max; + this->declare_parameter("destination_frame", "cart_frame"); + this->declare_parameter("cloud_destination_topic", "/merged_cloud"); + this->declare_parameter("scan_destination_topic", "/scan_multi"); + this->declare_parameter("laserscan_topics", ""); + this->declare_parameter("angle_min", -3.14); + this->declare_parameter("angle_max", 3.14); + this->declare_parameter("angle_increment", 0.0058); + this->declare_parameter("scan_time", 0.0); + this->declare_parameter("range_min", 0.0); + this->declare_parameter("range_max", 25.0); + + this->get_parameter("destination_frame", destination_frame); + this->get_parameter("cloud_destination_topic", cloud_destination_topic); + this->get_parameter("scan_destination_topic", scan_destination_topic); + this->get_parameter("laserscan_topics", laserscan_topics); + this->get_parameter("angle_min", angle_min); + this->get_parameter("angle_max", angle_max); + this->get_parameter("angle_increment", angle_increment); + this->get_parameter("scan_time", scan_time); + this->get_parameter("range_min", range_min); + this->get_parameter("range_max", range_max); + + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&LaserscanMerger::reconfigureCallback, this, _1)); + + tf_buffer_ = std::make_unique(this->get_clock()); + tfListener_ = std::make_shared(*tf_buffer_); + + this->laserscan_topic_parser(); + + point_cloud_publisher_ = this->create_publisher(cloud_destination_topic.c_str(), rclcpp::SensorDataQoS()); + laser_scan_publisher_ = this->create_publisher(scan_destination_topic.c_str(), rclcpp::SensorDataQoS()); +} + +rcl_interfaces::msg::SetParametersResult LaserscanMerger::reconfigureCallback(const std::vector ¶meters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) + { + const auto &type = parameter.get_type(); + const auto &name = parameter.get_name(); + + // Make sure it is a double value + if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) + { + double value = parameter.as_double(); + + if (name == "angle_min") + { + this->angle_min = value; + } + else if (name == "angle_max") + { + this->angle_max = value; + } + else if (name == "angle_increment") + { + this->angle_increment = value; + } + else if (name == "time_increment") + { + this->time_increment = value; + } + else if (name == "scan_time") + { + this->scan_time = value; + } + else if (name == "range_min") + { + this->range_min = value; + } + else if (name == "range_max") + { + this->range_max = value; + } + } + } + + result.successful = true; + return result; } void LaserscanMerger::laserscan_topic_parser() { // LaserScan topics to subscribe - ros::master::V_TopicInfo topics; + std::map> topics; istringstream iss(laserscan_topics); set tokens; copy(istream_iterator(iss), istream_iterator(), inserter>(tokens, tokens.begin())); - vector tmp_input_topics; + std::vector tmp_input_topics; while (!tokens.empty()) { - ROS_INFO("Waiting for topics ..."); - ros::master::getTopics(topics); + RCLCPP_INFO(this->get_logger(), "Waiting for topics ..."); sleep(1); - for (int i = 0; i < topics.size(); i++) + topics = this->get_topic_names_and_types(); + + for (const auto &topic_it : topics) { - if (topics[i].datatype == "sensor_msgs/LaserScan" && tokens.erase(topics[i].name) > 0) + std::vector topic_types = topic_it.second; + + if (std::find(topic_types.begin(), topic_types.end(), "sensor_msgs/msg/LaserScan") != topic_types.end() && tokens.erase(topic_it.first) > 0) { - tmp_input_topics.push_back(topics[i].name); + tmp_input_topics.push_back(topic_it.first); } } } @@ -99,11 +181,6 @@ void LaserscanMerger::laserscan_topic_parser() // Do not re-subscribe if the topics are the same if ((tmp_input_topics.size() != input_topics.size()) || !equal(tmp_input_topics.begin(), tmp_input_topics.end(), input_topics.begin())) { - - // Unsubscribe from previous topics - for (int i = 0; i < scan_subscribers.size(); i++) - scan_subscribers[i].shutdown(); - input_topics = tmp_input_topics; if (input_topics.size() > 0) @@ -111,73 +188,60 @@ void LaserscanMerger::laserscan_topic_parser() scan_subscribers.resize(input_topics.size()); clouds_modified.resize(input_topics.size()); clouds.resize(input_topics.size()); - ROS_INFO("Subscribing to topics\t%ld", scan_subscribers.size()); - for (int i = 0; i < input_topics.size(); ++i) + RCLCPP_INFO(this->get_logger(), "Subscribing to topics\t%ld", scan_subscribers.size()); + for (std::vector::size_type i = 0; i < input_topics.size(); ++i) { - scan_subscribers[i] = node_.subscribe(input_topics[i].c_str(), 1, boost::bind(&LaserscanMerger::scanCallback, this, _1, input_topics[i])); + // workaround for std::bind https://github.com/ros2/rclcpp/issues/583 + std::function callback = + std::bind( + &LaserscanMerger::scanCallback, + this, std::placeholders::_1, input_topics[i]); + scan_subscribers[i] = this->create_subscription(input_topics[i].c_str(), rclcpp::SensorDataQoS(), callback); clouds_modified[i] = false; cout << input_topics[i] << " "; } } else - ROS_INFO("Not subscribed to any topic."); + { + RCLCPP_INFO(this->get_logger(), "Not subscribed to any topic."); + } } } -LaserscanMerger::LaserscanMerger() -{ - ros::NodeHandle nh("~"); - - nh.param("destination_frame", destination_frame, "cart_frame"); - nh.param("cloud_destination_topic", cloud_destination_topic, "/merged_cloud"); - nh.param("scan_destination_topic", scan_destination_topic, "/scan_multi"); - nh.param("laserscan_topics", laserscan_topics, ""); - nh.param("angle_min", angle_min, -2.36); - nh.param("angle_max", angle_max, 2.36); - nh.param("angle_increment", angle_increment, 0.0058); - nh.param("scan_time", scan_time, 0.0333333); - nh.param("range_min", range_min, 0.45); - nh.param("range_max", range_max, 25.0); - - this->laserscan_topic_parser(); - - point_cloud_publisher_ = node_.advertise(cloud_destination_topic.c_str(), 1, false); - laser_scan_publisher_ = node_.advertise(scan_destination_topic.c_str(), 1, false); -} - -void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan, std::string topic) +void LaserscanMerger::scanCallback(sensor_msgs::msg::LaserScan::SharedPtr scan, std::string topic) { - sensor_msgs::PointCloud tmpCloud1, tmpCloud2; - sensor_msgs::PointCloud2 tmpCloud3; + sensor_msgs::msg::PointCloud2 tmpCloud1, tmpCloud2; - // refer to http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C%2B%2B%29 try { // Verify that TF knows how to transform from the received scan to the destination scan frame - tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1)); - projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_, laser_geometry::channel_option::Distance); - tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2); + tf_buffer_->lookupTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, rclcpp::Duration(1, 0)); + projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, *tf_buffer_, laser_geometry::channel_option::Distance); + pcl_ros::transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2, *tf_buffer_); } - catch (tf::TransformException ex) + catch (tf2::TransformException &ex) { return; } - for (int i = 0; i < input_topics.size(); i++) + for (std::vector::size_type i = 0; i < input_topics.size(); i++) { if (topic.compare(input_topics[i]) == 0) { - sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2, tmpCloud3); - pcl_conversions::toPCL(tmpCloud3, clouds[i]); + pcl_conversions::toPCL(tmpCloud2, clouds[i]); clouds_modified[i] = true; } } // Count how many scans we have - int totalClouds = 0; - for (int i = 0; i < clouds_modified.size(); i++) + std::vector::size_type totalClouds = 0; + for (std::vector::size_type i = 0; i < clouds_modified.size(); i++) + { if (clouds_modified[i]) + { totalClouds++; + } + } // Go ahead only if all subscribed scans have arrived if (totalClouds == clouds_modified.size()) @@ -185,39 +249,47 @@ void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan, pcl::PCLPointCloud2 merged_cloud = clouds[0]; clouds_modified[0] = false; - for (int i = 1; i < clouds_modified.size(); i++) + for (std::vector::size_type i = 1; i < clouds_modified.size(); i++) { - #if PCL_VERSION_COMPARE(>=, 1, 10, 0) - pcl::concatenate(merged_cloud, clouds[i], merged_cloud); - #else - pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud); - #endif - clouds_modified[i] = false; - } +#if PCL_VERSION_COMPARE(>=, 1, 10, 0) + merged_cloud += clouds[i]; +#else + pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud); +#endif - point_cloud_publisher_.publish(merged_cloud); + clouds_modified[i] = false; + } Eigen::MatrixXf points; - getPointCloudAsEigen(merged_cloud, points); + + pcl::getPointCloudAsEigen(merged_cloud, points); pointcloud_to_laserscan(points, &merged_cloud); + + // Publish point cloud after publishing laser scan as for some reason moveFromPCL is causing getPointCloudAsEigen to + // throw a segmentation fault crash + std::shared_ptr cloud_msg = std::make_shared(); + + pcl_conversions::moveFromPCL(merged_cloud, *cloud_msg); + + point_cloud_publisher_->publish(*cloud_msg); } } void LaserscanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud) { - sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan()); - output->header = pcl_conversions::fromPCL(merged_cloud->header); - output->angle_min = this->angle_min; - output->angle_max = this->angle_max; - output->angle_increment = this->angle_increment; - output->time_increment = this->time_increment; - output->scan_time = this->scan_time; - output->range_min = this->range_min; - output->range_max = this->range_max; - - uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment); - output->ranges.assign(ranges_size, output->range_max + 1.0); + sensor_msgs::msg::LaserScan output; + output.header = pcl_conversions::fromPCL(merged_cloud->header); + output.angle_min = this->angle_min; + output.angle_max = this->angle_max; + output.angle_increment = this->angle_increment; + output.time_increment = this->time_increment; + output.scan_time = this->scan_time; + output.range_min = this->range_min; + output.range_max = this->range_max; + + uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); + output.ranges.assign(ranges_size, std::numeric_limits::infinity()); for (int i = 0; i < points.cols(); i++) { @@ -227,46 +299,42 @@ void LaserscanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPo if (std::isnan(x) || std::isnan(y) || std::isnan(z)) { - ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); + RCLCPP_DEBUG(this->get_logger(), "rejected for nan in point(%f, %f, %f)\n", x, y, z); continue; } double range_sq = pow(y, 2) + pow(x, 2); - double range_min_sq_ = output->range_min * output->range_min; + double range_min_sq_ = output.range_min * output.range_min; if (range_sq < range_min_sq_) { - ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z); + RCLCPP_DEBUG(this->get_logger(), "rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z); continue; } double angle = atan2(y, x); - if (angle < output->angle_min || angle > output->angle_max) + if (angle < output.angle_min || angle > output.angle_max) { - ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max); + RCLCPP_DEBUG(this->get_logger(), "rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); continue; } - int index = (angle - output->angle_min) / output->angle_increment; - if (output->ranges[index] * output->ranges[index] > range_sq) - output->ranges[index] = sqrt(range_sq); + int index = (angle - output.angle_min) / output.angle_increment; + if (output.ranges[index] * output.ranges[index] > range_sq) + { + output.ranges[index] = sqrt(range_sq); + } } - laser_scan_publisher_.publish(output); + laser_scan_publisher_->publish(output); } int main(int argc, char **argv) { - ros::init(argc, argv, "laser_multi_merger"); - - LaserscanMerger _laser_merger; - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + rclcpp::init(argc, argv); - f = boost::bind(&LaserscanMerger::reconfigureCallback, &_laser_merger, _1, _2); - server.setCallback(f); + rclcpp::spin(std::make_shared()); - ros::spin(); + rclcpp::shutdown(); return 0; } diff --git a/src/laserscan_virtualizer.cpp b/src/laserscan_virtualizer.cpp index 1cad5f5..3308af5 100644 --- a/src/laserscan_virtualizer.cpp +++ b/src/laserscan_virtualizer.cpp @@ -1,42 +1,44 @@ -#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include -#include "sensor_msgs/LaserScan.h" -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" +#include +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "pcl_ros/transforms.hpp" typedef pcl::PointCloud myPointCloud; using namespace std; using namespace pcl; -using namespace laserscan_virtualizer; -class LaserscanVirtualizer +using std::placeholders::_1; + +class LaserscanVirtualizer : public rclcpp::Node { public: LaserscanVirtualizer(); - void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan, std::string topic); void pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLHeader scan_header, int pub_index); - void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &pcl_in); - void reconfigureCallback(laserscan_virtualizerConfig &config, uint32_t level); + void pointCloudCallback(sensor_msgs::msg::PointCloud2::SharedPtr pcl_in); + rcl_interfaces::msg::SetParametersResult reconfigureCallback(const std::vector ¶meters); private: - ros::NodeHandle node_; - tf::TransformListener tfListener_; - vector transform_; + std::shared_ptr tfListener_; + std::unique_ptr tf_buffer_; + std::vector> transform_; + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - ros::Subscriber point_cloud_subscriber_; - vector virtual_scan_publishers; - vector output_frames; + rclcpp::Subscription::SharedPtr point_cloud_subscription_; + std::vector::SharedPtr> virtual_scan_publishers; + std::vector output_frames; void virtual_laser_scan_parser(); @@ -55,37 +57,77 @@ class LaserscanVirtualizer string virtual_laser_scan; }; -void LaserscanVirtualizer::reconfigureCallback(laserscan_virtualizerConfig &config, uint32_t level) +rcl_interfaces::msg::SetParametersResult LaserscanVirtualizer::reconfigureCallback(const std::vector ¶meters) { - this->angle_min = config.angle_min; - this->angle_max = config.angle_max; - this->angle_increment = config.angle_increment; - this->time_increment = config.time_increment; - this->scan_time = config.scan_time; - this->range_min = config.range_min; - this->range_max = config.range_max; + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) + { + const auto &type = parameter.get_type(); + const auto &name = parameter.get_name(); + + // Make sure it is a double value + if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) + { + double value = parameter.as_double(); + + if (name == "angle_min") + { + this->angle_min = value; + } + else if (name == "angle_max") + { + this->angle_max = value; + } + else if (name == "angle_increment") + { + this->angle_increment = value; + } + else if (name == "time_increment") + { + this->time_increment = value; + } + else if (name == "scan_time") + { + this->scan_time = value; + } + else if (name == "range_min") + { + this->range_min = value; + } + else if (name == "range_max") + { + this->range_max = value; + } + } + } + + result.successful = true; + return result; } void LaserscanVirtualizer::virtual_laser_scan_parser() { // LaserScan frames to use for virtualization istringstream iss(virtual_laser_scan); - vector tokens; - copy(istream_iterator(iss), istream_iterator(), back_inserter>(tokens)); + std::vector tokens; + copy(istream_iterator(iss), istream_iterator(), back_inserter>(tokens)); - vector tmp_output_frames; + std::vector tmp_output_frames; - for (int i = 0; i < tokens.size(); i++) + for (std::vector::size_type i = 0; i < tokens.size(); i++) { - ros::Time beg = ros::Time::now(); - if (tfListener_.waitForTransform(base_frame, tokens[i], ros::Time(0), ros::Duration(1.0))) // Check if TF knows the transform from this frame reference to base_frame reference + auto beg = this->get_clock()->now(); + if (tf_buffer_->canTransform(base_frame, tokens[i], rclcpp::Time(0), rclcpp::Duration(1, 0))) // Check if TF knows the transform from this frame reference to base_frame reference { - cout << "Elapsed: " << ros::Time::now() - beg << endl; + cout << "Elapsed: " << (this->get_clock()->now() - beg).nanoseconds() / 1e9 << endl; cout << "Adding: " << tokens[i] << endl; tmp_output_frames.push_back(tokens[i]); } else + { cout << "Can't transform: '" << tokens[i] + "' to '" << base_frame << "'" << endl; + } } // Sort and remove duplicates @@ -96,69 +138,90 @@ void LaserscanVirtualizer::virtual_laser_scan_parser() // Do not re-advertize if the topics are the same if ((tmp_output_frames.size() != output_frames.size()) || !equal(tmp_output_frames.begin(), tmp_output_frames.end(), output_frames.begin())) { - // Shutdown previous publishers - for (int i = 0; i < virtual_scan_publishers.size(); i++) - virtual_scan_publishers[i].shutdown(); - cloud_frame = ""; output_frames = tmp_output_frames; if (output_frames.size() > 0) { virtual_scan_publishers.resize(output_frames.size()); - ROS_INFO("Publishing: %ld virtual scans", virtual_scan_publishers.size()); + RCLCPP_INFO(this->get_logger(), "Publishing: %ld virtual scans", virtual_scan_publishers.size()); cout << "Advertising topics: " << endl; - for (int i = 0; i < output_frames.size(); ++i) + for (std::vector::size_type i = 0; i < output_frames.size(); ++i) { if (output_laser_topic.empty()) { - virtual_scan_publishers[i] = node_.advertise(output_frames[i].c_str(), 1); + virtual_scan_publishers[i] = this->create_publisher(output_frames[i].c_str(), rclcpp::SensorDataQoS()); cout << "\t\t" << output_frames[i] << " on topic " << output_frames[i].c_str() << endl; } else { - virtual_scan_publishers[i] = node_.advertise(output_laser_topic.c_str(), 1); + virtual_scan_publishers[i] = this->create_publisher(output_laser_topic.c_str(), rclcpp::SensorDataQoS()); cout << "\t\t" << output_frames[i] << " on topic " << output_laser_topic.c_str() << endl; } } } else - ROS_INFO("Not publishing to any topic."); + { + RCLCPP_INFO(this->get_logger(), "Not publishing to any topic."); + } } } -LaserscanVirtualizer::LaserscanVirtualizer() +LaserscanVirtualizer::LaserscanVirtualizer() : Node("laserscan_virtualizer") { - ros::NodeHandle nh("~"); - - //Setting class parameters - if (!nh.getParam("/laserscan_virtualizer/base_frame", base_frame)) - base_frame = "/cart_frame"; - - if (!nh.getParam("/laserscan_virtualizer/cloud_topic", cloud_topic)) - cloud_topic = "/cloud_pcd"; - - nh.getParam("/laserscan_virtualizer/output_laser_topic", output_laser_topic); - nh.getParam("/laserscan_virtualizer/virtual_laser_scan", virtual_laser_scan); + this->declare_parameter("base_frame", "base_link"); + this->declare_parameter("cloud_topic", "/cloud_pcd"); + this->declare_parameter("output_laser_topic", "/scan"); + this->declare_parameter("virtual_laser_scan", "scansx scandx"); + this->declare_parameter("angle_min", -3.14); + this->declare_parameter("angle_max", 3.14); + this->declare_parameter("angle_increment", 0.0058); + this->declare_parameter("scan_time", 0.0); + this->declare_parameter("range_min", 0.0); + this->declare_parameter("range_max", 25.0); + + this->get_parameter("base_frame", base_frame); + this->get_parameter("cloud_topic", cloud_topic); + this->get_parameter("output_laser_topic", output_laser_topic); + this->get_parameter("virtual_laser_scan", virtual_laser_scan); + this->get_parameter("angle_min", angle_min); + this->get_parameter("angle_max", angle_max); + this->get_parameter("angle_increment", angle_increment); + this->get_parameter("scan_time", scan_time); + this->get_parameter("range_min", range_min); + this->get_parameter("range_max", range_max); + + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&LaserscanVirtualizer::reconfigureCallback, this, _1)); + + tf_buffer_ = std::make_unique(this->get_clock()); + tfListener_ = std::make_shared(*tf_buffer_); this->virtual_laser_scan_parser(); - point_cloud_subscriber_ = node_.subscribe(cloud_topic.c_str(), 1, boost::bind(&LaserscanVirtualizer::pointCloudCallback, this, _1)); + point_cloud_subscription_ = + this->create_subscription(cloud_topic.c_str(), rclcpp::SensorDataQoS(), std::bind(&LaserscanVirtualizer::pointCloudCallback, this, _1)); cloud_frame = ""; } -void LaserscanVirtualizer::pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &pcl_in) +void LaserscanVirtualizer::pointCloudCallback(sensor_msgs::msg::PointCloud2::SharedPtr pcl_in) { if (cloud_frame.empty()) { cloud_frame = (*pcl_in).header.frame_id; transform_.resize(output_frames.size()); - for (int i = 0; i < output_frames.size(); i++) - if (tfListener_.waitForTransform(output_frames[i], cloud_frame, ros::Time(0), ros::Duration(2.0))) - tfListener_.lookupTransform(output_frames[i], cloud_frame, ros::Time(0), transform_[i]); + for (std::vector::size_type i = 0; i < output_frames.size(); i++) + { + if (tf_buffer_->canTransform(output_frames[i], cloud_frame, rclcpp::Time(0), rclcpp::Duration(2, 0))) + { + geometry_msgs::msg::TransformStamped tfGeom = tf_buffer_->lookupTransform(output_frames[i], cloud_frame, rclcpp::Time(0)); + + tf2::convert(tfGeom, transform_[i]); + } + } } - for (int i = 0; i < output_frames.size(); i++) + for (std::vector::size_type i = 0; i < output_frames.size(); i++) { myPointCloud pcl_out, tmpPcl; pcl::PCLPointCloud2 tmpPcl2; @@ -185,20 +248,20 @@ void LaserscanVirtualizer::pointCloudCallback(const sensor_msgs::PointCloud2::Co } } -void LaserscanVirtualizer::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLHeader scan_header, int pub_index) //pcl::PCLPointCloud2 *merged_cloud) +void LaserscanVirtualizer::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLHeader scan_header, int pub_index) // pcl::PCLPointCloud2 *merged_cloud) { - sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan()); - output->header = pcl_conversions::fromPCL(scan_header); - output->angle_min = this->angle_min; - output->angle_max = this->angle_max; - output->angle_increment = this->angle_increment; - output->time_increment = this->time_increment; - output->scan_time = this->scan_time; - output->range_min = this->range_min; - output->range_max = this->range_max; - - uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment); - output->ranges.assign(ranges_size, output->range_max + 1.0); + sensor_msgs::msg::LaserScan output; + output.header = pcl_conversions::fromPCL(scan_header); + output.angle_min = this->angle_min; + output.angle_max = this->angle_max; + output.angle_increment = this->angle_increment; + output.time_increment = this->time_increment; + output.scan_time = this->scan_time; + output.range_min = this->range_min; + output.range_max = this->range_max; + + uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); + output.ranges.assign(ranges_size, output.range_max + 1.0); for (int i = 0; i < points.cols(); i++) { @@ -208,51 +271,42 @@ void LaserscanVirtualizer::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl:: if (std::isnan(x) || std::isnan(y) || std::isnan(z)) { - ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); + RCLCPP_DEBUG(this->get_logger(), "rejected for nan in point(%f, %f, %f)\n", x, y, z); continue; } - pcl::PointXYZ p; - p.x = x; - p.y = y; - p.z = z; - double range_sq = pow(y, 2) + pow(x, 2); - double range_min_sq_ = output->range_min * output->range_min; + double range_min_sq_ = output.range_min * output.range_min; if (range_sq < range_min_sq_) { - ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z); + RCLCPP_DEBUG(this->get_logger(), "rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z); continue; } double angle = atan2(y, x); - if (angle < output->angle_min || angle > output->angle_max) + if (angle < output.angle_min || angle > output.angle_max) { - ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max); + RCLCPP_DEBUG(this->get_logger(), "rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max); continue; } - int index = (angle - output->angle_min) / output->angle_increment; - if (output->ranges[index] * output->ranges[index] > range_sq) - output->ranges[index] = sqrt(range_sq); + int index = (angle - output.angle_min) / output.angle_increment; + if (output.ranges[index] * output.ranges[index] > range_sq) + { + output.ranges[index] = sqrt(range_sq); + } } - virtual_scan_publishers[pub_index].publish(output); + virtual_scan_publishers[pub_index]->publish(output); } int main(int argc, char **argv) { - ros::init(argc, argv, "laserscan_virtualizer"); - - LaserscanVirtualizer _laser_merger; - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + rclcpp::init(argc, argv); - f = boost::bind(&LaserscanVirtualizer::reconfigureCallback, &_laser_merger, _1, _2); - server.setCallback(f); + rclcpp::spin(std::make_shared()); - ros::spin(); + rclcpp::shutdown(); return 0; }