Use upstream pointcloud_to_laserscan
This commit is contained in:
Matt Spencer
@@ -1,95 +0,0 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package pointcloud_to_laserscan
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
2.0.1 (2021-10-19)
|
||||
------------------
|
||||
* Replace deprecated launch api (`#56 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/56>`_)
|
||||
* Fix linting errors in the code (`#52 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/52>`_)
|
||||
* Update README.md
|
||||
* Contributors: Chris Lalancette, Daisuke Nishimatsu, Paul Bovbel
|
||||
|
||||
2.0.0 (2020-02-10)
|
||||
------------------
|
||||
* ROS 2 Migration (`#33 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/33>`_)
|
||||
* ROS 2 Migration
|
||||
Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
|
||||
* Contributors: Michel Hidalgo
|
||||
|
||||
1.4.1 (2019-08-30)
|
||||
------------------
|
||||
* LaserScan to PointCloud node + nodelet (`#28 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/28>`_)
|
||||
* fix roslint (`#29 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/29>`_)
|
||||
* Merge pull request `#20 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/20>`_ from ros-perception/range_max_check
|
||||
Add check for range_max
|
||||
* Add check for range_max
|
||||
* Contributors: Paul Bovbel, Rein Appeldoorn
|
||||
|
||||
1.4.0 (2017-11-14)
|
||||
------------------
|
||||
* Added inf_epsilon parameter that determines the value added to max range when use_infs parameter is set to false
|
||||
* Merge pull request `#11 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/11>`_ from ros-perception/mikaelarguedas-patch-1
|
||||
update to use non deprecated pluginlib macro
|
||||
* Migrate to package format 2; add roslint
|
||||
* Add sane parameter defaults; fixup lint warnings
|
||||
* update to use non deprecated pluginlib macro
|
||||
* Contributors: Mikael Arguedas, Paul Bovbel, Prasanna Kannappan
|
||||
|
||||
1.3.1 (2017-04-26)
|
||||
------------------
|
||||
* Merge pull request `#4 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/4>`_ from yoshimalucky/fix-miscalculation-in-angle-increment
|
||||
Fixed miscalculation in angle_increment in the launch files.
|
||||
* fixed miscalculation in angle_increment in the launchfiles.
|
||||
* Contributors: Paul Bovbel, yoshimalucky
|
||||
|
||||
1.3.0 (2015-06-09)
|
||||
------------------
|
||||
* Fix pointcloud to laserscan transform tolerance issues
|
||||
* Move pointcloud_to_laserscan to new repository
|
||||
* Contributors: Paul Bovbel
|
||||
|
||||
1.2.7 (2015-06-08)
|
||||
------------------
|
||||
|
||||
* Cleanup pointcloud_to_laserscan launch files
|
||||
* Contributors: Paul Bovbel
|
||||
|
||||
1.2.6 (2015-02-04)
|
||||
------------------
|
||||
* Fix default value for concurrency
|
||||
* Fix multithreaded lazy pub sub
|
||||
* Contributors: Paul Bovbel
|
||||
|
||||
1.2.5 (2015-01-20)
|
||||
------------------
|
||||
* Switch to tf_sensor_msgs for transform
|
||||
* Set parameters in sample launch files to default
|
||||
* Add tolerance parameter
|
||||
* Contributors: Paul Bovbel
|
||||
|
||||
1.2.4 (2015-01-15)
|
||||
------------------
|
||||
* Remove stray dependencies
|
||||
* Refactor with tf2 and message filters
|
||||
* Remove roslaunch check
|
||||
* Fix regressions
|
||||
* Refactor to allow debug messages from node and nodelet
|
||||
* Contributors: Paul Bovbel
|
||||
|
||||
1.2.3 (2015-01-10)
|
||||
------------------
|
||||
* add launch tests
|
||||
* refactor naming and fix nodelet export
|
||||
* set default target frame to empty
|
||||
* clean up package.xml
|
||||
* Contributors: Paul Bovbel
|
||||
|
||||
1.2.2 (2014-10-25)
|
||||
------------------
|
||||
* clean up package.xml
|
||||
* Fix header reference
|
||||
* Fix flow
|
||||
* Fix pointer assertion
|
||||
* Finalize pointcloud to laserscan
|
||||
* Initial pointcloud to laserscan commit
|
||||
* Contributors: Paul Bovbel
|
||||
@@ -1,33 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(pointcloud_to_laserscan)
|
||||
|
||||
find_package(ament_cmake_auto REQUIRED)
|
||||
ament_auto_find_build_dependencies()
|
||||
|
||||
ament_auto_add_library(laserscan_to_pointcloud SHARED
|
||||
src/laserscan_to_pointcloud_node.cpp)
|
||||
|
||||
rclcpp_components_register_node(laserscan_to_pointcloud
|
||||
PLUGIN "pointcloud_to_laserscan::LaserScanToPointCloudNode"
|
||||
EXECUTABLE laserscan_to_pointcloud_node)
|
||||
|
||||
ament_auto_add_library(pointcloud_to_laserscan SHARED
|
||||
src/pointcloud_to_laserscan_node.cpp)
|
||||
|
||||
rclcpp_components_register_node(pointcloud_to_laserscan
|
||||
PLUGIN "pointcloud_to_laserscan::PointCloudToLaserScanNode"
|
||||
EXECUTABLE pointcloud_to_laserscan_node)
|
||||
|
||||
ament_auto_add_executable(dummy_pointcloud_publisher
|
||||
src/dummy_pointcloud_publisher.cpp
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_auto_package(
|
||||
INSTALL_TO_SHARE
|
||||
launch
|
||||
)
|
||||
@@ -1,12 +0,0 @@
|
||||
Copyright (c) 2010-2012, Willow Garage, Inc.
|
||||
Copyright (c) 2019, Eurotec, Netherlands
|
||||
|
||||
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.
|
||||
@@ -1,49 +0,0 @@
|
||||
# ROS 2 pointcloud <-> laserscan converters
|
||||
|
||||
This is a ROS 2 package that provides components to convert `sensor_msgs/msg/PointCloud2` messages to `sensor_msgs/msg/LaserScan` messages and back.
|
||||
It is essentially a port of the original ROS 1 package.
|
||||
|
||||
## pointcloud\_to\_laserscan::PointCloudToLaserScanNode
|
||||
|
||||
This ROS 2 component projects `sensor_msgs/msg/PointCloud2` messages into `sensor_msgs/msg/LaserScan` messages.
|
||||
|
||||
### Published Topics
|
||||
|
||||
* `scan` (`sensor_msgs/msg/LaserScan`) - The output laser scan.
|
||||
|
||||
### Subscribed Topics
|
||||
|
||||
* `cloud_in` (`sensor_msgs/msg/PointCloud2`) - The input point cloud. No input will be processed if there isn't at least one subscriber to the `scan` topic.
|
||||
|
||||
### Parameters
|
||||
|
||||
* `min_height` (double, default: 2.2e-308) - The minimum height to sample in the point cloud in meters.
|
||||
* `max_height` (double, default: 1.8e+308) - The maximum height to sample in the point cloud in meters.
|
||||
* `angle_min` (double, default: -π) - The minimum scan angle in radians.
|
||||
* `angle_max` (double, default: π) - The maximum scan angle in radians.
|
||||
* `angle_increment` (double, default: π/180) - Resolution of laser scan in radians per ray.
|
||||
* `queue_size` (double, default: detected number of cores) - Input point cloud queue size.
|
||||
* `scan_time` (double, default: 1.0/30.0) - The scan rate in seconds. Only used to populate the scan_time field of the output laser scan message.
|
||||
* `range_min` (double, default: 0.0) - The minimum ranges to return in meters.
|
||||
* `range_max` (double, default: 1.8e+308) - The maximum ranges to return in meters.
|
||||
* `target_frame` (str, default: none) - If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud.
|
||||
* `transform_tolerance` (double, default: 0.01) - Time tolerance for transform lookups. Only used if a `target_frame` is provided.
|
||||
* `use_inf` (boolean, default: true) - If disabled, report infinite range (no obstacle) as range_max + 1. Otherwise report infinite range as +inf.
|
||||
|
||||
## pointcloud\_to\_laserscan::LaserScanToPointCloudNode
|
||||
|
||||
This ROS 2 component re-publishes `sensor_msgs/msg/LaserScan` messages as `sensor_msgs/msg/PointCloud2` messages.
|
||||
|
||||
### Published Topics
|
||||
|
||||
* `cloud` (`sensor_msgs/msg/PointCloud2`) - The output point cloud.
|
||||
|
||||
### Subscribed Topics
|
||||
|
||||
* `scan_in` (`sensor_msgs/msg/LaserScan`) - The input laser scan. No input will be processed if there isn't at least one subscriber to the `cloud` topic.
|
||||
|
||||
### Parameters
|
||||
|
||||
* `queue_size` (double, default: detected number of cores) - Input laser scan queue size.
|
||||
* `target_frame` (str, default: none) - If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud.
|
||||
* `transform_tolerance` (double, default: 0.01) - Time tolerance for transform lookups. Only used if a `target_frame` is provided.
|
||||
-98
@@ -1,98 +0,0 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2019, Eurotec, Netherlands
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * 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.
|
||||
* * Neither the name of Willow Garage, Inc. 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 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.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Author: Rein Appeldoorn
|
||||
*/
|
||||
|
||||
#ifndef POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
|
||||
#define POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "message_filters/subscriber.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/message_filter.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
#include "laser_geometry/laser_geometry.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
|
||||
#include "pointcloud_to_laserscan/visibility_control.h"
|
||||
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
typedef tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan> MessageFilter;
|
||||
|
||||
//! \brief The PointCloudToLaserScanNodelet class to process incoming laserscans into pointclouds.
|
||||
//!
|
||||
class LaserScanToPointCloudNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
explicit LaserScanToPointCloudNode(const rclcpp::NodeOptions & options);
|
||||
|
||||
~LaserScanToPointCloudNode() override;
|
||||
|
||||
private:
|
||||
void scanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg);
|
||||
|
||||
void subscriptionListenerThreadLoop();
|
||||
|
||||
std::unique_ptr<tf2_ros::Buffer> tf2_;
|
||||
std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::LaserScan> sub_;
|
||||
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_;
|
||||
std::unique_ptr<MessageFilter> message_filter_;
|
||||
std::thread subscription_listener_thread_;
|
||||
std::atomic_bool alive_{true};
|
||||
|
||||
laser_geometry::LaserProjection projector_;
|
||||
|
||||
// ROS Parameters
|
||||
int input_queue_size_;
|
||||
std::string target_frame_;
|
||||
double tolerance_;
|
||||
};
|
||||
|
||||
} // namespace pointcloud_to_laserscan
|
||||
|
||||
#endif // POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
|
||||
-102
@@ -1,102 +0,0 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010-2012, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * 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.
|
||||
* * Neither the name of Willow Garage, Inc. 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 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.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Author: Paul Bovbel
|
||||
*/
|
||||
|
||||
#ifndef POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
|
||||
#define POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "message_filters/subscriber.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/message_filter.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
|
||||
#include "pointcloud_to_laserscan/visibility_control.h"
|
||||
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
typedef tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2> MessageFilter;
|
||||
|
||||
/**
|
||||
* Class to process incoming pointclouds into laserscans.
|
||||
* Some initial code was pulled from the defunct turtlebot pointcloud_to_laserscan implementation.
|
||||
*/
|
||||
class PointCloudToLaserScanNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
explicit PointCloudToLaserScanNode(const rclcpp::NodeOptions & options);
|
||||
|
||||
~PointCloudToLaserScanNode() override;
|
||||
|
||||
private:
|
||||
void cloudCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg);
|
||||
|
||||
void subscriptionListenerThreadLoop();
|
||||
|
||||
std::unique_ptr<tf2_ros::Buffer> tf2_;
|
||||
std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> sub_;
|
||||
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> pub_;
|
||||
std::unique_ptr<MessageFilter> message_filter_;
|
||||
|
||||
std::thread subscription_listener_thread_;
|
||||
std::atomic_bool alive_{true};
|
||||
|
||||
// ROS Parameters
|
||||
int input_queue_size_;
|
||||
std::string target_frame_;
|
||||
double tolerance_;
|
||||
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_,
|
||||
range_max_;
|
||||
bool use_inf_;
|
||||
double inf_epsilon_;
|
||||
};
|
||||
|
||||
} // namespace pointcloud_to_laserscan
|
||||
|
||||
#endif // POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
|
||||
@@ -1,80 +0,0 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2019, Eurotec, Netherlands
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * 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.
|
||||
* * Neither the name of Willow Garage, Inc. 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 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.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
|
||||
#define POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define POINTCLOUD_TO_LASERSCAN_EXPORT __attribute__ ((dllexport))
|
||||
#define POINTCLOUD_TO_LASERSCAN_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_EXPORT __declspec(dllexport)
|
||||
#define POINTCLOUD_TO_LASERSCAN_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef POINTCLOUD_TO_LASERSCAN_BUILDING_DLL
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC POINTCLOUD_TO_LASERSCAN_EXPORT
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC POINTCLOUD_TO_LASERSCAN_IMPORT
|
||||
#endif
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC_TYPE POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
#define POINTCLOUD_TO_LASERSCAN_LOCAL
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_EXPORT __attribute__ ((visibility("default")))
|
||||
#define POINTCLOUD_TO_LASERSCAN_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define POINTCLOUD_TO_LASERSCAN_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
#define POINTCLOUD_TO_LASERSCAN_LOCAL
|
||||
#endif
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
|
||||
@@ -1,43 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='scanner', default_value='unilidar',
|
||||
description='Namespace for Unitree LiDAR topics, used to organize topic names'
|
||||
),
|
||||
|
||||
# Make sure the Unitree LiDAR node is already running
|
||||
# and publishing point cloud data to the /unilidar/cloud topic
|
||||
|
||||
# ========== Unitree PointCloud to LaserScan Converter ==========
|
||||
# Convert Unitree LiDAR 3D point cloud data into 2D LaserScan data
|
||||
Node(
|
||||
package='pointcloud_to_laserscan',
|
||||
executable='pointcloud_to_laserscan_node',
|
||||
|
||||
remappings=[
|
||||
('cloud_in', '/unilidar/cloud'), # Input: Unitree LiDAR point cloud topic
|
||||
('scan', '/scan') # Output: standard LaserScan topic
|
||||
],
|
||||
parameters=[{
|
||||
'min_height': 0.35, # Minimum height: filter points below this height
|
||||
'max_height': 0.45, # Maximum height: keep obstacles, filter ceiling points
|
||||
'angle_min': -3.14159, # Minimum scan angle: -π radians (-180 degrees)
|
||||
'angle_max': 3.14159, # Maximum scan angle: π radians (180 degrees)
|
||||
'angle_increment': 0.00436, # Angular resolution: π/720 radians (~0.25 degrees)
|
||||
'scan_time': 0.1, # Scan time: 0.1s (10 Hz publishing rate, suitable for navigation)
|
||||
'range_min': 0.1, # Minimum valid range: avoid self-reflections
|
||||
'range_max': 100.0, # Maximum valid range: Unitree LiDAR effective range
|
||||
'use_inf': True, # Use infinity for missing readings (LaserScan standard)
|
||||
'inf_epsilon': 1.0 # Epsilon value for infinity handling
|
||||
}],
|
||||
name='unitree_pointcloud_to_laserscan' # Node name
|
||||
)
|
||||
])
|
||||
|
||||
@@ -1,47 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.actions import ExecuteProcess
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
name='scanner', default_value='scanner',
|
||||
description='Namespace for sample topics'
|
||||
),
|
||||
ExecuteProcess(
|
||||
cmd=[
|
||||
'ros2', 'topic', 'pub', '-r', '10',
|
||||
'--qos-profile', 'sensor_data',
|
||||
[LaunchConfiguration(variable_name='scanner'), '/scan'],
|
||||
'sensor_msgs/msg/LaserScan', yaml.dump({
|
||||
'header': {'frame_id': 'scan'}, 'angle_min': -1.0,
|
||||
'angle_max': 1.0, 'angle_increment': 0.1, 'range_max': 10.0,
|
||||
'ranges': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
|
||||
})
|
||||
],
|
||||
name='scan_publisher'
|
||||
),
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
name='static_transform_publisher',
|
||||
arguments=[
|
||||
'--x', '0', '--y', '0', '--z', '0',
|
||||
'--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
|
||||
'--frame-id', 'map', '--child-frame-id', 'scan'
|
||||
]
|
||||
),
|
||||
Node(
|
||||
package='pointcloud_to_laserscan',
|
||||
executable='laserscan_to_pointcloud_node',
|
||||
name='laserscan_to_pointcloud',
|
||||
remappings=[('scan_in', [LaunchConfiguration(variable_name='scanner'), '/scan']),
|
||||
('cloud', [LaunchConfiguration(variable_name='scanner'), '/cloud'])],
|
||||
parameters=[{'target_frame': 'scan', 'transform_tolerance': 0.01}]
|
||||
),
|
||||
])
|
||||
@@ -1,49 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
name='scanner', default_value='scanner',
|
||||
description='Namespace for sample topics'
|
||||
),
|
||||
Node(
|
||||
package='pointcloud_to_laserscan', executable='dummy_pointcloud_publisher',
|
||||
remappings=[('cloud', [LaunchConfiguration(variable_name='scanner'), '/cloud'])],
|
||||
parameters=[{'cloud_frame_id': 'cloud', 'cloud_extent': 2.0, 'cloud_size': 500}],
|
||||
name='cloud_publisher'
|
||||
),
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
name='static_transform_publisher',
|
||||
arguments=[
|
||||
'--x', '0', '--y', '0', '--z', '0',
|
||||
'--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
|
||||
'--frame-id', 'map', '--child-frame-id', 'cloud'
|
||||
]
|
||||
),
|
||||
Node(
|
||||
package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
|
||||
remappings=[('cloud_in', [LaunchConfiguration(variable_name='scanner'), '/cloud']),
|
||||
('scan', [LaunchConfiguration(variable_name='scanner'), '/scan'])],
|
||||
parameters=[{
|
||||
'target_frame': 'cloud',
|
||||
'transform_tolerance': 0.01,
|
||||
'min_height': 0.0,
|
||||
'max_height': 1.0,
|
||||
'angle_min': -1.5708, # -M_PI/2
|
||||
'angle_max': 1.5708, # M_PI/2
|
||||
'angle_increment': 0.0087, # M_PI/360.0
|
||||
'scan_time': 0.3333,
|
||||
'range_min': 0.45,
|
||||
'range_max': 4.0,
|
||||
'use_inf': True,
|
||||
'inf_epsilon': 1.0
|
||||
}],
|
||||
name='pointcloud_to_laserscan'
|
||||
)
|
||||
])
|
||||
@@ -1,46 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>pointcloud_to_laserscan</name>
|
||||
<version>2.0.1</version>
|
||||
<description>Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM).</description>
|
||||
|
||||
<maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
|
||||
<maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
|
||||
<author email="paul@bovbel.com">Paul Bovbel</author>
|
||||
<author email="michel@ekumenlabs.com">Michel Hidalgo</author>
|
||||
<author>Tully Foote</author>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://ros.org/wiki/perception_pcl</url>
|
||||
<url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
|
||||
<url type="repository">https://github.com/ros-perception/perception_pcl</url>
|
||||
|
||||
<buildtool_depend>ament_cmake_auto</buildtool_depend>
|
||||
|
||||
<depend>laser_geometry</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_sensor_msgs</depend>
|
||||
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<!-- <exec_depend>ament_cmake_copyright</exec_depend> -->
|
||||
<test_depend>ament_cmake_cppcheck</test_depend>
|
||||
<test_depend>ament_cmake_cpplint</test_depend>
|
||||
<test_depend>ament_cmake_flake8</test_depend>
|
||||
<test_depend>ament_cmake_lint_cmake</test_depend>
|
||||
<test_depend>ament_cmake_pep257</test_depend>
|
||||
<test_depend>ament_cmake_uncrustify</test_depend>
|
||||
<test_depend>ament_cmake_xmllint</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,85 +0,0 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2020, Open Source Robotics Foundation, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * 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.
|
||||
* * Neither the name of Willow Garage, Inc. 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 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 <memory>
|
||||
#include <random>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
#include "sensor_msgs/point_cloud2_iterator.hpp"
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<rclcpp::Node>("dummy_pointcloud_publisher");
|
||||
auto pub =
|
||||
node->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS());
|
||||
|
||||
sensor_msgs::msg::PointCloud2 dummy_cloud;
|
||||
sensor_msgs::PointCloud2Modifier modifier(dummy_cloud);
|
||||
modifier.setPointCloud2Fields(
|
||||
3,
|
||||
"x", 1, sensor_msgs::msg::PointField::FLOAT32,
|
||||
"y", 1, sensor_msgs::msg::PointField::FLOAT32,
|
||||
"z", 1, sensor_msgs::msg::PointField::FLOAT32);
|
||||
modifier.resize(node->declare_parameter("cloud_size", 100));
|
||||
std::mt19937 gen(node->declare_parameter("cloud_seed", 0));
|
||||
double extent = node->declare_parameter("cloud_extent", 10.0);
|
||||
std::uniform_real_distribution<float> distribution(-extent / 2, extent / 2);
|
||||
sensor_msgs::PointCloud2Iterator<float> it_x(dummy_cloud, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> it_y(dummy_cloud, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> it_z(dummy_cloud, "z");
|
||||
for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) {
|
||||
*it_x = distribution(gen);
|
||||
*it_y = distribution(gen);
|
||||
*it_z = distribution(gen);
|
||||
}
|
||||
dummy_cloud.header.frame_id = node->declare_parameter("cloud_frame_id", "");
|
||||
|
||||
rclcpp::executors::SingleThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
rclcpp::Rate rate(1.0);
|
||||
while (rclcpp::ok()) {
|
||||
dummy_cloud.header.stamp = node->get_clock()->now();
|
||||
pub->publish(dummy_cloud);
|
||||
executor.spin_some();
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -1,150 +0,0 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2019, Eurotec, Netherlands
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * 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.
|
||||
* * Neither the name of Willow Garage, Inc. 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 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.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Author: Rein Appeldoorn
|
||||
*/
|
||||
|
||||
#include "pointcloud_to_laserscan/laserscan_to_pointcloud_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "sensor_msgs/point_cloud2_iterator.hpp"
|
||||
#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
|
||||
#include "tf2_ros/create_timer_ros.h"
|
||||
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
|
||||
LaserScanToPointCloudNode::LaserScanToPointCloudNode(const rclcpp::NodeOptions & options)
|
||||
: rclcpp::Node("laserscan_to_pointcloud", options)
|
||||
{
|
||||
target_frame_ = this->declare_parameter("target_frame", "");
|
||||
tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
|
||||
// TODO(hidmic): adjust default input queue size based on actual concurrency levels
|
||||
// achievable by the associated executor
|
||||
input_queue_size_ = this->declare_parameter(
|
||||
"queue_size", static_cast<int>(std::thread::hardware_concurrency()));
|
||||
|
||||
pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS());
|
||||
|
||||
using std::placeholders::_1;
|
||||
// if pointcloud target frame specified, we need to filter by transform availability
|
||||
if (!target_frame_.empty()) {
|
||||
tf2_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
this->get_node_base_interface(), this->get_node_timers_interface());
|
||||
tf2_->setCreateTimerInterface(timer_interface);
|
||||
tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_);
|
||||
message_filter_ = std::make_unique<MessageFilter>(
|
||||
sub_, *tf2_, target_frame_, input_queue_size_,
|
||||
this->get_node_logging_interface(),
|
||||
this->get_node_clock_interface());
|
||||
message_filter_->registerCallback(
|
||||
std::bind(
|
||||
&LaserScanToPointCloudNode::scanCallback, this, _1));
|
||||
} else { // otherwise setup direct subscription
|
||||
sub_.registerCallback(std::bind(&LaserScanToPointCloudNode::scanCallback, this, _1));
|
||||
}
|
||||
|
||||
subscription_listener_thread_ = std::thread(
|
||||
std::bind(&LaserScanToPointCloudNode::subscriptionListenerThreadLoop, this));
|
||||
}
|
||||
|
||||
LaserScanToPointCloudNode::~LaserScanToPointCloudNode()
|
||||
{
|
||||
alive_.store(true);
|
||||
subscription_listener_thread_.join();
|
||||
}
|
||||
|
||||
void LaserScanToPointCloudNode::subscriptionListenerThreadLoop()
|
||||
{
|
||||
rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
|
||||
|
||||
const std::chrono::milliseconds timeout(100);
|
||||
while (rclcpp::ok(context) && alive_.load()) {
|
||||
int subscription_count = pub_->get_subscription_count() +
|
||||
pub_->get_intra_process_subscription_count();
|
||||
if (subscription_count > 0) {
|
||||
if (!sub_.getSubscriber()) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Got a subscriber to pointcloud, starting laserscan subscriber");
|
||||
rclcpp::SensorDataQoS qos;
|
||||
qos.keep_last(input_queue_size_);
|
||||
sub_.subscribe(this, "scan_in", qos.get_rmw_qos_profile());
|
||||
}
|
||||
} else if (sub_.getSubscriber()) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"No subscribers to pointcloud, shutting down laserscan subscriber");
|
||||
sub_.unsubscribe();
|
||||
}
|
||||
rclcpp::Event::SharedPtr event = this->get_graph_event();
|
||||
this->wait_for_graph_change(event, timeout);
|
||||
}
|
||||
sub_.unsubscribe();
|
||||
}
|
||||
|
||||
void LaserScanToPointCloudNode::scanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg)
|
||||
{
|
||||
auto cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
|
||||
|
||||
projector_.projectLaser(*scan_msg, *cloud_msg);
|
||||
|
||||
// Transform cloud if necessary
|
||||
if (!target_frame_.empty() && cloud_msg->header.frame_id != target_frame_) {
|
||||
try {
|
||||
*cloud_msg = tf2_->transform(*cloud_msg, target_frame_, tf2::durationFromSec(tolerance_));
|
||||
} catch (tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(), "Transform failure: " << ex.what());
|
||||
return;
|
||||
}
|
||||
}
|
||||
pub_->publish(std::move(cloud_msg));
|
||||
}
|
||||
|
||||
} // namespace pointcloud_to_laserscan
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_to_laserscan::LaserScanToPointCloudNode)
|
||||
@@ -1,242 +0,0 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010-2012, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * 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.
|
||||
* * Neither the name of Willow Garage, Inc. 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 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.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Author: Paul Bovbel
|
||||
*/
|
||||
|
||||
#include "pointcloud_to_laserscan/pointcloud_to_laserscan_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "sensor_msgs/point_cloud2_iterator.hpp"
|
||||
#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
|
||||
#include "tf2_ros/create_timer_ros.h"
|
||||
#include "rmw/qos_profiles.h"
|
||||
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
|
||||
PointCloudToLaserScanNode::PointCloudToLaserScanNode(const rclcpp::NodeOptions & options)
|
||||
: rclcpp::Node("pointcloud_to_laserscan", options)
|
||||
{
|
||||
target_frame_ = this->declare_parameter("target_frame", "");
|
||||
tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
|
||||
// TODO(hidmic): adjust default input queue size based on actual concurrency levels
|
||||
// achievable by the associated executor
|
||||
input_queue_size_ = this->declare_parameter(
|
||||
"queue_size", static_cast<int>(std::thread::hardware_concurrency()));
|
||||
min_height_ = this->declare_parameter("min_height", std::numeric_limits<double>::min());
|
||||
max_height_ = this->declare_parameter("max_height", std::numeric_limits<double>::max());
|
||||
angle_min_ = this->declare_parameter("angle_min", -M_PI);
|
||||
angle_max_ = this->declare_parameter("angle_max", M_PI);
|
||||
angle_increment_ = this->declare_parameter("angle_increment", M_PI / 180.0);
|
||||
scan_time_ = this->declare_parameter("scan_time", 1.0 / 30.0);
|
||||
range_min_ = this->declare_parameter("range_min", 0.0);
|
||||
range_max_ = this->declare_parameter("range_max", std::numeric_limits<double>::max());
|
||||
inf_epsilon_ = this->declare_parameter("inf_epsilon", 1.0);
|
||||
use_inf_ = this->declare_parameter("use_inf", true);
|
||||
|
||||
// Create a laser scan publisher, using BEST_EFFORT QoS to accommodate more subscribers.
|
||||
rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
|
||||
qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
|
||||
pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", qos_profile);
|
||||
|
||||
using std::placeholders::_1;
|
||||
// if pointcloud target frame specified, we need to filter by transform availability
|
||||
if (!target_frame_.empty()) {
|
||||
tf2_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
this->get_node_base_interface(), this->get_node_timers_interface());
|
||||
tf2_->setCreateTimerInterface(timer_interface);
|
||||
tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_);
|
||||
message_filter_ = std::make_unique<MessageFilter>(
|
||||
sub_, *tf2_, target_frame_, input_queue_size_,
|
||||
this->get_node_logging_interface(),
|
||||
this->get_node_clock_interface());
|
||||
message_filter_->registerCallback(
|
||||
std::bind(&PointCloudToLaserScanNode::cloudCallback, this, _1));
|
||||
} else { // otherwise setup direct subscription
|
||||
sub_.registerCallback(std::bind(&PointCloudToLaserScanNode::cloudCallback, this, _1));
|
||||
}
|
||||
|
||||
subscription_listener_thread_ = std::thread(
|
||||
std::bind(&PointCloudToLaserScanNode::subscriptionListenerThreadLoop, this));
|
||||
}
|
||||
|
||||
PointCloudToLaserScanNode::~PointCloudToLaserScanNode()
|
||||
{
|
||||
alive_.store(false);
|
||||
subscription_listener_thread_.join();
|
||||
}
|
||||
|
||||
void PointCloudToLaserScanNode::subscriptionListenerThreadLoop()
|
||||
{
|
||||
rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
|
||||
|
||||
const std::chrono::milliseconds timeout(100);
|
||||
while (rclcpp::ok(context) && alive_.load()) {
|
||||
int subscription_count = pub_->get_subscription_count() +
|
||||
pub_->get_intra_process_subscription_count();
|
||||
if (subscription_count > 0) {
|
||||
if (!sub_.getSubscriber()) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Got a subscriber to laserscan, starting pointcloud subscriber");
|
||||
rclcpp::SensorDataQoS qos;
|
||||
qos.keep_last(input_queue_size_);
|
||||
sub_.subscribe(this, "cloud_in", qos.get_rmw_qos_profile());
|
||||
}
|
||||
} else if (sub_.getSubscriber()) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"No subscribers to laserscan, shutting down pointcloud subscriber");
|
||||
sub_.unsubscribe();
|
||||
}
|
||||
rclcpp::Event::SharedPtr event = this->get_graph_event();
|
||||
this->wait_for_graph_change(event, timeout);
|
||||
}
|
||||
sub_.unsubscribe();
|
||||
}
|
||||
|
||||
void PointCloudToLaserScanNode::cloudCallback(
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg)
|
||||
{
|
||||
// build laserscan output
|
||||
auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
|
||||
scan_msg->header = cloud_msg->header;
|
||||
if (!target_frame_.empty()) {
|
||||
scan_msg->header.frame_id = target_frame_;
|
||||
}
|
||||
|
||||
scan_msg->angle_min = angle_min_;
|
||||
scan_msg->angle_max = angle_max_;
|
||||
scan_msg->angle_increment = angle_increment_;
|
||||
scan_msg->time_increment = 0.0;
|
||||
scan_msg->scan_time = scan_time_;
|
||||
scan_msg->range_min = range_min_;
|
||||
scan_msg->range_max = range_max_;
|
||||
|
||||
// determine amount of rays to create
|
||||
uint32_t ranges_size = std::ceil(
|
||||
(scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
|
||||
|
||||
// determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
|
||||
if (use_inf_) {
|
||||
scan_msg->ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
|
||||
} else {
|
||||
scan_msg->ranges.assign(ranges_size, scan_msg->range_max + inf_epsilon_);
|
||||
}
|
||||
|
||||
// Transform cloud if necessary
|
||||
if (scan_msg->header.frame_id != cloud_msg->header.frame_id) {
|
||||
try {
|
||||
auto cloud = std::make_shared<sensor_msgs::msg::PointCloud2>();
|
||||
tf2_->transform(*cloud_msg, *cloud, target_frame_, tf2::durationFromSec(tolerance_));
|
||||
cloud_msg = cloud;
|
||||
} catch (tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(), "Transform failure: " << ex.what());
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Iterate through pointcloud
|
||||
for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x"),
|
||||
iter_y(*cloud_msg, "y"), iter_z(*cloud_msg, "z");
|
||||
iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
{
|
||||
if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"rejected for nan in point(%f, %f, %f)\n",
|
||||
*iter_x, *iter_y, *iter_z);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (*iter_z > max_height_ || *iter_z < min_height_) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"rejected for height %f not in range (%f, %f)\n",
|
||||
*iter_z, min_height_, max_height_);
|
||||
continue;
|
||||
}
|
||||
|
||||
double range = hypot(*iter_x, *iter_y);
|
||||
if (range < range_min_) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"rejected for range %f below minimum value %f. Point: (%f, %f, %f)",
|
||||
range, range_min_, *iter_x, *iter_y, *iter_z);
|
||||
continue;
|
||||
}
|
||||
if (range > range_max_) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"rejected for range %f above maximum value %f. Point: (%f, %f, %f)",
|
||||
range, range_max_, *iter_x, *iter_y, *iter_z);
|
||||
continue;
|
||||
}
|
||||
|
||||
double angle = atan2(*iter_y, *iter_x);
|
||||
if (angle < scan_msg->angle_min || angle > scan_msg->angle_max) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"rejected for angle %f not in range (%f, %f)\n",
|
||||
angle, scan_msg->angle_min, scan_msg->angle_max);
|
||||
continue;
|
||||
}
|
||||
|
||||
// overwrite range at laserscan ray if new range is smaller
|
||||
int index = (angle - scan_msg->angle_min) / scan_msg->angle_increment;
|
||||
if (range < scan_msg->ranges[index]) {
|
||||
scan_msg->ranges[index] = range;
|
||||
}
|
||||
}
|
||||
pub_->publish(std::move(scan_msg));
|
||||
}
|
||||
|
||||
} // namespace pointcloud_to_laserscan
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_to_laserscan::PointCloudToLaserScanNode)
|
||||
|
||||
@@ -19,6 +19,10 @@ repositories:
|
||||
type: git
|
||||
url: https://github.com/dfloreaa/point_lio_ros2.git
|
||||
revision: main
|
||||
pointcloud_to_laserscan:
|
||||
type: git
|
||||
url: https://github.com/ros-perception/pointcloud_to_laserscan.git
|
||||
revision: jazzy
|
||||
slam_gmapping:
|
||||
type: git
|
||||
url: https://github.com/Project-MANAS/slam_gmapping.git
|
||||
|
||||
Reference in New Issue
Block a user