diff --git a/src/ira_laser_tools b/src/ira_laser_tools deleted file mode 160000 index 958d0ac..0000000 --- a/src/ira_laser_tools +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 958d0ac8e103822ce9ce04c0bb9c3b499322575d diff --git a/src/ira_laser_tools/CHANGELOG.rst b/src/ira_laser_tools/CHANGELOG.rst new file mode 100644 index 0000000..9a502c5 --- /dev/null +++ b/src/ira_laser_tools/CHANGELOG.rst @@ -0,0 +1,77 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ira_laser_tools +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.7 (2021-10-30) +------------------ +* Use the same header in published scan as the published cloud +* Fix a concurrency issue +* Contributors: JackFrost67, MikHut, Auri + +1.0.6 (2021-08-12) +------------------ +* add check for PCL version +* add new mantainer email +* fix launch before laser scan is available +* add a try catch for first non valid tf +* Retrying compare topics between ROS master and Token +* Contributors: JackFrost67 + +1.0.4 (2020-02-14) +------------------ +* fix worning + ":0:0: warning: missing whitespace after the macro name" +* change email maintainer +* Contributors: Pietro Colombo + +1.0.3 (2020-02-13) +------------------ +* fix `#14 `_ + now we use math library for pi +* bugfix +* Merge branch 'kinetic' +* fix issue fix`#14 `_ +* Create LICENSE +* Create LICENSE +* Merge pull request `#11 `_ from mikaelarguedas/patch-1 + add libvtk-qt dependency to fix debian stretch builds +* add libvtk-qt dependency to fix debian stretch builds +* Contributors: Mikael Arguedas, Pietro Colombo, pietrocolombo + +1.0.2 (2018-08-28) +------------------ +* add libvtk-qt dependency to fix debian stretch builds + and link to paper in README.md +* Contributors: Pietro Colombo + +1.0.1 (2018-07-18) +------------------ + +1.0.0 (2018-07-11 10:30:16 +0200) +--------------------------------- +* add url for wiki in pakage.xml +* adding license +* introduce a param in param file + expand the range limit of laser + and minor bug fix +* Merge pull request `#1 `_ from leonziegler/installprocedure + Added possibility to install artifacts. + Merged. Thanks :-) +* Merge branch 'master' of projects.ira.disco.unimib.it:/repository/git/ira_laser_tools + Conflicts: + CMakeLists.txt +* this commit fixes `#265 `_ @5m +* Fixed the Eigen3 required in the CMakeLists.txt +* updating readme +* removing eigen dependency +* Update laserscan_virtualizer.launch + description of base_frame param. updated +* Added possibility to install artifacts. +* changes to README.md +* global refactoring +* Merge branch 'master' of https://github.com/iralabdisco/ira_laser_tools +* going to version 1.0; useful tutorial on .launch file for laservirtualizer; the overall virtualscanner procedure is now simpler +* First README.md version +* adding readme file +* Fixed the include bug +* First commit +* Contributors: Augusto Luis Ballardini, Axel Furlan, Fabio Nava, Iralab Universita Milano Bicocca, Leon Ziegler diff --git a/src/ira_laser_tools/CMakeLists.txt b/src/ira_laser_tools/CMakeLists.txt new file mode 100644 index 0000000..6d35f93 --- /dev/null +++ b/src/ira_laser_tools/CMakeLists.txt @@ -0,0 +1,153 @@ +cmake_minimum_required(VERSION 3.0.2) +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) + +find_package(Eigen3 REQUIRED) + +find_package(PCL REQUIRED) + +# 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) + +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 + + +############# +## 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} +) + +## 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 +) + +############# +## 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) diff --git a/src/ira_laser_tools/LICENSE b/src/ira_laser_tools/LICENSE new file mode 100644 index 0000000..5a27518 --- /dev/null +++ b/src/ira_laser_tools/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2019, Iralab +All rights reserved. + +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. diff --git a/src/ira_laser_tools/README.md b/src/ira_laser_tools/README.md new file mode 100644 index 0000000..2b7ac96 --- /dev/null +++ b/src/ira_laser_tools/README.md @@ -0,0 +1,30 @@ +This library includes some tools for laser handling in ROS. At the moment two nodes are available: + + - laserscan_multi_merger + - laserscan_virtualizer + +Both use part of the pointcloud_to_laserscan code available in ROS. + +Laserscan_merger allows to easily and dynamically (rqt_reconfigure) merge multiple, same time, +single scanning plane, laser scans into a single one; this is very useful for using applications +like gmapping, amcl, pamcl on vehicles with multiple single scanning plane laser scanners, as these +applications require just one laser scan as input. The scanning planes need to be approximately the +same. The resulting scan will appear generated from a single scanner dis-regarding actual +occlusions as seen from the merged scans; for instance, consider the case of 2 scanners mounted on +the 2 front corners A and B of a rectangular vehicle (we live in 2D); each scanner gives out a +270degs scan, from along the long side of the vehicle going backward to toward the other scanner. +The merged scan will appear as generated from a virtual scanner positioned in C, halfway A and B, +and measuring the same measures of the merges scans, irregardless of the occlusions that would +apply to a real scanner positioned in C. + +Laserscan_virtualizer allows to easily and dynamically (rqt_reconfifure) generate virtual laser +scans from a pointcloud such as the one generated by a multiple scanning plane laser scanner, e.g., +a velodyne scanner). The only requirement is the rototranslation between the virtual laser scanner +and the base frame to be known to TF. + +The documentation is at the moment very brief, for any question please contact us at +furlan@disco.unimib.it or augusto.ballardini@unimib.it + +Paper link: https://arxiv.org/abs/1411.1086 + +Both nodes compile under catkin in hydro/indigo and use PCL1.7 diff --git a/src/ira_laser_tools/cfg/laserscan_multi_merger.cfg b/src/ira_laser_tools/cfg/laserscan_multi_merger.cfg new file mode 100755 index 0000000..5705022 --- /dev/null +++ b/src/ira_laser_tools/cfg/laserscan_multi_merger.cfg @@ -0,0 +1,17 @@ +#!/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/src/ira_laser_tools/cfg/laserscan_virtualizer.cfg b/src/ira_laser_tools/cfg/laserscan_virtualizer.cfg new file mode 100755 index 0000000..9610084 --- /dev/null +++ b/src/ira_laser_tools/cfg/laserscan_virtualizer.cfg @@ -0,0 +1,17 @@ +#!/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/src/ira_laser_tools/launch/laserscan_multi_merger.launch b/src/ira_laser_tools/launch/laserscan_multi_merger.launch new file mode 100644 index 0000000..e94127e --- /dev/null +++ b/src/ira_laser_tools/launch/laserscan_multi_merger.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/ira_laser_tools/launch/laserscan_virtualizer.launch b/src/ira_laser_tools/launch/laserscan_virtualizer.launch new file mode 100644 index 0000000..4b3b91b --- /dev/null +++ b/src/ira_laser_tools/launch/laserscan_virtualizer.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + diff --git a/src/ira_laser_tools/package.xml b/src/ira_laser_tools/package.xml new file mode 100644 index 0000000..f4f7098 --- /dev/null +++ b/src/ira_laser_tools/package.xml @@ -0,0 +1,34 @@ + + + ira_laser_tools + 1.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 + + + libvtk-qt + libvtk-qt + + diff --git a/src/ira_laser_tools/src/laserscan_multi_merger.cpp b/src/ira_laser_tools/src/laserscan_multi_merger.cpp new file mode 100644 index 0000000..c5428f6 --- /dev/null +++ b/src/ira_laser_tools/src/laserscan_multi_merger.cpp @@ -0,0 +1,272 @@ +#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 + +using namespace std; +using namespace pcl; +using namespace laserscan_multi_merger; + +class LaserscanMerger +{ + 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; +}; + +void LaserscanMerger::reconfigureCallback(laserscan_multi_mergerConfig &config, uint32_t level) +{ + 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; +} + +void LaserscanMerger::laserscan_topic_parser() +{ + // LaserScan topics to subscribe + ros::master::V_TopicInfo topics; + + istringstream iss(laserscan_topics); + set tokens; + copy(istream_iterator(iss), istream_iterator(), inserter>(tokens, tokens.begin())); + vector tmp_input_topics; + + while (!tokens.empty()) + { + ROS_INFO("Waiting for topics ..."); + ros::master::getTopics(topics); + sleep(1); + + for (int i = 0; i < topics.size(); i++) + { + if (topics[i].datatype == "sensor_msgs/LaserScan" && tokens.erase(topics[i].name) > 0) + { + tmp_input_topics.push_back(topics[i].name); + } + } + } + + sort(tmp_input_topics.begin(), tmp_input_topics.end()); + std::vector::iterator last = std::unique(tmp_input_topics.begin(), tmp_input_topics.end()); + tmp_input_topics.erase(last, tmp_input_topics.end()); + + // 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) + { + 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) + { + scan_subscribers[i] = node_.subscribe(input_topics[i].c_str(), 1, boost::bind(&LaserscanMerger::scanCallback, this, _1, input_topics[i])); + clouds_modified[i] = false; + cout << input_topics[i] << " "; + } + } + else + ROS_INFO("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) +{ + sensor_msgs::PointCloud tmpCloud1, tmpCloud2; + sensor_msgs::PointCloud2 tmpCloud3; + + // 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); + } + catch (tf::TransformException ex) + { + return; + } + + for (int 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]); + clouds_modified[i] = true; + } + } + + // Count how many scans we have + int totalClouds = 0; + for (int 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()) + { + pcl::PCLPointCloud2 merged_cloud = clouds[0]; + clouds_modified[0] = false; + + for (int 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; + } + + point_cloud_publisher_.publish(merged_cloud); + + Eigen::MatrixXf points; + getPointCloudAsEigen(merged_cloud, points); + + pointcloud_to_laserscan(points, &merged_cloud); + } +} + +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); + + for (int i = 0; i < points.cols(); i++) + { + const float &x = points(0, i); + const float &y = points(1, i); + const float &z = points(2, i); + + if (std::isnan(x) || std::isnan(y) || std::isnan(z)) + { + ROS_DEBUG("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; + 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); + continue; + } + + double angle = atan2(y, x); + 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); + 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); + } + + 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; + + f = boost::bind(&LaserscanMerger::reconfigureCallback, &_laser_merger, _1, _2); + server.setCallback(f); + + ros::spin(); + + return 0; +} diff --git a/src/ira_laser_tools/src/laserscan_virtualizer.cpp b/src/ira_laser_tools/src/laserscan_virtualizer.cpp new file mode 100644 index 0000000..1cad5f5 --- /dev/null +++ b/src/ira_laser_tools/src/laserscan_virtualizer.cpp @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "sensor_msgs/LaserScan.h" +#include +#include +#include +#include + +typedef pcl::PointCloud myPointCloud; + +using namespace std; +using namespace pcl; +using namespace laserscan_virtualizer; + +class LaserscanVirtualizer +{ +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); + +private: + ros::NodeHandle node_; + tf::TransformListener tfListener_; + vector transform_; + + ros::Subscriber point_cloud_subscriber_; + vector virtual_scan_publishers; + vector output_frames; + + void virtual_laser_scan_parser(); + + double angle_min; + double angle_max; + double angle_increment; + double time_increment; + double scan_time; + double range_min; + double range_max; + + string cloud_frame; + string base_frame; + string cloud_topic; + string output_laser_topic; + string virtual_laser_scan; +}; + +void LaserscanVirtualizer::reconfigureCallback(laserscan_virtualizerConfig &config, uint32_t level) +{ + 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; +} + +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)); + + vector tmp_output_frames; + + for (int 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 + { + cout << "Elapsed: " << ros::Time::now() - beg << 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 + sort(tmp_output_frames.begin(), tmp_output_frames.end()); + std::vector::iterator last = std::unique(tmp_output_frames.begin(), tmp_output_frames.end()); + tmp_output_frames.erase(last, tmp_output_frames.end()); + + // 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()); + cout << "Advertising topics: " << endl; + for (int i = 0; i < output_frames.size(); ++i) + { + if (output_laser_topic.empty()) + { + virtual_scan_publishers[i] = node_.advertise(output_frames[i].c_str(), 1); + 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); + cout << "\t\t" << output_frames[i] << " on topic " << output_laser_topic.c_str() << endl; + } + } + } + else + ROS_INFO("Not publishing to any topic."); + } +} + +LaserscanVirtualizer::LaserscanVirtualizer() +{ + 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->virtual_laser_scan_parser(); + + point_cloud_subscriber_ = node_.subscribe(cloud_topic.c_str(), 1, boost::bind(&LaserscanVirtualizer::pointCloudCallback, this, _1)); + cloud_frame = ""; +} + +void LaserscanVirtualizer::pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &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 (int i = 0; i < output_frames.size(); i++) + { + myPointCloud pcl_out, tmpPcl; + pcl::PCLPointCloud2 tmpPcl2; + + pcl_conversions::toPCL(*pcl_in, tmpPcl2); + pcl::fromPCLPointCloud2(tmpPcl2, tmpPcl); + + // Initialize the header of the temporary pointcloud, needed to rototranslate the three points that define our plane + // It shall be equal to the input point cloud's one, changing only the 'frame_id' + string tmpFrame = output_frames[i]; + pcl_out.header = tmpPcl.header; + + // Ask tf to rototranslate the velodyne pointcloud to base_frame reference + pcl_ros::transformPointCloud(tmpPcl, pcl_out, transform_[i]); + pcl_out.header.frame_id = tmpFrame; + + // Transform the pcl into eigen matrix + Eigen::MatrixXf tmpEigenMatrix; + pcl::toPCLPointCloud2(pcl_out, tmpPcl2); + pcl::getPointCloudAsEigen(tmpPcl2, tmpEigenMatrix); + + // Extract the points close to the z=0 plane, convert them into a laser-scan message and publish it + pointcloud_to_laserscan(tmpEigenMatrix, pcl_out.header, i); + } +} + +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); + + for (int i = 0; i < points.cols(); i++) + { + const float &x = points(0, i); + const float &y = points(1, i); + const float &z = points(2, i); + + if (std::isnan(x) || std::isnan(y) || std::isnan(z)) + { + ROS_DEBUG("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; + 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); + continue; + } + + double angle = atan2(y, x); + 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); + 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); + } + + 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; + + f = boost::bind(&LaserscanVirtualizer::reconfigureCallback, &_laser_merger, _1, _2); + server.setCallback(f); + + ros::spin(); + + return 0; +}