diff --git a/velodyne/launch/velodyne-all-nodes-VLP16-composed-launch.py b/velodyne/launch/velodyne-all-nodes-VLP16-composed-launch.py index ea6754628..a9b5219d5 100644 --- a/velodyne/launch/velodyne-all-nodes-VLP16-composed-launch.py +++ b/velodyne/launch/velodyne-all-nodes-VLP16-composed-launch.py @@ -47,9 +47,9 @@ def generate_launch_description(): driver_params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters'] convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_convert_node-params.yaml') + convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_transform_node-params.yaml') with open(convert_params_file, 'r') as f: - convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] + convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLP16db.yaml') laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan') @@ -70,8 +70,8 @@ def generate_launch_description(): parameters=[driver_params]), ComposableNode( package='velodyne_pointcloud', - plugin='velodyne_pointcloud::Convert', - name='velodyne_convert_node', + plugin='velodyne_pointcloud::Transform', + name='velodyne_transform_node', parameters=[convert_params]), ComposableNode( package='velodyne_laserscan', diff --git a/velodyne/launch/velodyne-all-nodes-VLP16-launch.py b/velodyne/launch/velodyne-all-nodes-VLP16-launch.py index db5978703..45fd41d7a 100644 --- a/velodyne/launch/velodyne-all-nodes-VLP16-launch.py +++ b/velodyne/launch/velodyne-all-nodes-VLP16-launch.py @@ -48,14 +48,14 @@ def generate_launch_description(): parameters=[driver_params_file]) convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_convert_node-params.yaml') + convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_transform_node-params.yaml') with open(convert_params_file, 'r') as f: - convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] + convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLP16db.yaml') - velodyne_convert_node = launch_ros.actions.Node(package='velodyne_pointcloud', - executable='velodyne_convert_node', - output='both', - parameters=[convert_params]) + velodyne_transform_node = launch_ros.actions.Node(package='velodyne_pointcloud', + executable='velodyne_transform_node', + output='both', + parameters=[convert_params]) laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan') laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml') @@ -66,7 +66,7 @@ def generate_launch_description(): return launch.LaunchDescription([velodyne_driver_node, - velodyne_convert_node, + velodyne_transform_node, velodyne_laserscan_node, launch.actions.RegisterEventHandler( diff --git a/velodyne/launch/velodyne-all-nodes-VLP32C-composed-launch.py b/velodyne/launch/velodyne-all-nodes-VLP32C-composed-launch.py index 1f9f2e768..438019c34 100644 --- a/velodyne/launch/velodyne-all-nodes-VLP32C-composed-launch.py +++ b/velodyne/launch/velodyne-all-nodes-VLP32C-composed-launch.py @@ -47,9 +47,9 @@ def generate_launch_description(): driver_params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters'] convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_convert_node-params.yaml') + convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_transform_node-params.yaml') with open(convert_params_file, 'r') as f: - convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] + convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VeloView-VLP-32C.yaml') laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan') @@ -70,8 +70,8 @@ def generate_launch_description(): parameters=[driver_params]), ComposableNode( package='velodyne_pointcloud', - plugin='velodyne_pointcloud::Convert', - name='velodyne_convert_node', + plugin='velodyne_pointcloud::Transform', + name='velodyne_transform_node', parameters=[convert_params]), ComposableNode( package='velodyne_laserscan', diff --git a/velodyne/launch/velodyne-all-nodes-VLP32C-launch.py b/velodyne/launch/velodyne-all-nodes-VLP32C-launch.py index e2209ab45..9eb100886 100644 --- a/velodyne/launch/velodyne-all-nodes-VLP32C-launch.py +++ b/velodyne/launch/velodyne-all-nodes-VLP32C-launch.py @@ -48,14 +48,14 @@ def generate_launch_description(): parameters=[driver_params_file]) convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_convert_node-params.yaml') + convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_transform_node-params.yaml') with open(convert_params_file, 'r') as f: - convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] + convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VeloView-VLP-32C.yaml') - velodyne_convert_node = launch_ros.actions.Node(package='velodyne_pointcloud', - executable='velodyne_convert_node', - output='both', - parameters=[convert_params]) + velodyne_transform_node = launch_ros.actions.Node(package='velodyne_pointcloud', + executable='velodyne_transform_node', + output='both', + parameters=[convert_params]) laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan') laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml') @@ -66,7 +66,7 @@ def generate_launch_description(): return launch.LaunchDescription([velodyne_driver_node, - velodyne_convert_node, + velodyne_transform_node, velodyne_laserscan_node, launch.actions.RegisterEventHandler( diff --git a/velodyne_driver/tests/pcap_32e_nodelet_hertz.test b/velodyne_driver/tests/pcap_32e_nodelet_hertz.test index 0047b4f18..c63ec784c 100644 --- a/velodyne_driver/tests/pcap_32e_nodelet_hertz.test +++ b/velodyne_driver/tests/pcap_32e_nodelet_hertz.test @@ -3,7 +3,7 @@ - + diff --git a/velodyne_driver/tests/pcap_nodelet_hertz.test b/velodyne_driver/tests/pcap_nodelet_hertz.test index c38d2bf8b..7ef3b126c 100644 --- a/velodyne_driver/tests/pcap_nodelet_hertz.test +++ b/velodyne_driver/tests/pcap_nodelet_hertz.test @@ -3,7 +3,7 @@ - + diff --git a/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test b/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test index 7c2f83193..1968149d6 100644 --- a/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test +++ b/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test @@ -3,7 +3,7 @@ - + diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index 540ae03ce..bdc6f8a8b 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -45,38 +45,6 @@ include_directories(include ${PCL_COMMON_INCLUDE_DIRS}) add_subdirectory(src/lib) -add_library(convert SHARED - src/conversions/convert.cpp) -ament_target_dependencies(convert - diagnostic_updater - Eigen3 - rclcpp - rclcpp_components - tf2 - velodyne_msgs -) -target_link_libraries(convert - velodyne_cloud_types - velodyne_rawdata - ${YAML_CPP_LIBRARIES}) -install(TARGETS convert - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -rclcpp_components_register_nodes(convert - "velodyne_pointcloud::Convert") - -add_executable(velodyne_convert_node - src/conversions/convert_node.cpp) -ament_target_dependencies(velodyne_convert_node - rclcpp -) -target_link_libraries(velodyne_convert_node convert) -install(TARGETS velodyne_convert_node - DESTINATION lib/${PROJECT_NAME} -) - add_library(transform SHARED src/conversions/transform.cpp) ament_target_dependencies(transform diff --git a/velodyne_pointcloud/config/VLP16-velodyne_convert_node-params.yaml b/velodyne_pointcloud/config/VLP16-velodyne_convert_node-params.yaml deleted file mode 100644 index af5d3d6d4..000000000 --- a/velodyne_pointcloud/config/VLP16-velodyne_convert_node-params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -velodyne_convert_node: - ros__parameters: - calibration: VLP16db.yaml - model: VLP16 - target_frame: velodyne - fixed_frame: velodyne - min_range: 0.9 - max_range: 130.0 - view_direction: 0.0 - organize_cloud: true diff --git a/velodyne_pointcloud/config/VLP16-velodyne_transform_node-params.yaml b/velodyne_pointcloud/config/VLP16-velodyne_transform_node-params.yaml index 45ec94835..38e86af3d 100644 --- a/velodyne_pointcloud/config/VLP16-velodyne_transform_node-params.yaml +++ b/velodyne_pointcloud/config/VLP16-velodyne_transform_node-params.yaml @@ -5,4 +5,6 @@ velodyne_transform_node: min_range: 0.9 max_range: 130.0 view_direction: 0.0 + fixed_frame: "" + target_frame: "" organize_cloud: true diff --git a/velodyne_pointcloud/config/VLP32C-velodyne_convert_node-params.yaml b/velodyne_pointcloud/config/VLP32C-velodyne_convert_node-params.yaml deleted file mode 100644 index 0990fc946..000000000 --- a/velodyne_pointcloud/config/VLP32C-velodyne_convert_node-params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -velodyne_convert_node: - ros__parameters: - calibration: VeloView-VLP-32C.yaml - model: 32C - target_frame: velodyne - fixed_frame: velodyne - min_range: 0.9 - max_range: 200.0 - view_direction: 0.0 - organize_cloud: true diff --git a/velodyne_pointcloud/config/VLP32C-velodyne_transform_node-params.yaml b/velodyne_pointcloud/config/VLP32C-velodyne_transform_node-params.yaml index f254824f4..d9b81cd84 100644 --- a/velodyne_pointcloud/config/VLP32C-velodyne_transform_node-params.yaml +++ b/velodyne_pointcloud/config/VLP32C-velodyne_transform_node-params.yaml @@ -5,4 +5,6 @@ velodyne_transform_node: min_range: 0.9 max_range: 200.0 view_direction: 0.0 + fixed_frame: "" + target_frame: "" organize_cloud: true diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/convert.hpp deleted file mode 100644 index 8de97194e..000000000 --- a/velodyne_pointcloud/include/velodyne_pointcloud/convert.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2009, 2010, 2011, 2012, 2019 Austin Robot Technology, Jack O'Quin, Jesse Vera, Joshua Whitley // NOLINT -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// -// 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 {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 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 VELODYNE_POINTCLOUD__CONVERT_HPP_ -#define VELODYNE_POINTCLOUD__CONVERT_HPP_ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "velodyne_pointcloud/pointcloudXYZIRT.hpp" -#include "velodyne_pointcloud/rawdata.hpp" - -namespace velodyne_pointcloud -{ -class Convert final - : public rclcpp::Node -{ -public: - explicit Convert(const rclcpp::NodeOptions & options); - ~Convert() override {} - Convert(Convert && c) = delete; - Convert & operator=(Convert && c) = delete; - Convert(const Convert & c) = delete; - Convert & operator=(const Convert & c) = delete; - -private: - void processScan(const velodyne_msgs::msg::VelodyneScan::SharedPtr scanMsg); - - std::unique_ptr data_; - rclcpp::Subscription::SharedPtr velodyne_scan_; - rclcpp::Publisher::SharedPtr output_; - tf2_ros::Buffer tf_buffer_; - std::unique_ptr container_ptr_; - - /// configuration parameters - struct Config - { - int npackets; // number of packets to combine - }; - Config config_{}; - - // diagnostics updater - diagnostic_updater::Updater diagnostics_; - double diag_min_freq_; - double diag_max_freq_; - std::unique_ptr diag_topic_; -}; -} // namespace velodyne_pointcloud - -#endif // VELODYNE_POINTCLOUD__CONVERT_HPP_ diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.hpp index 0508eba44..9f4b8b6ba 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.hpp @@ -34,12 +34,15 @@ #define VELODYNE_POINTCLOUD__DATACONTAINERBASE_HPP_ #include +#include #include #include #include #include #include #include +#include +#include #include #include @@ -58,11 +61,11 @@ class DataContainerBase explicit DataContainerBase( const double min_range, const double max_range, const std::string & target_frame, const std::string & fixed_frame, const unsigned int init_width, const unsigned int init_height, - const bool is_dense, const unsigned int scans_per_packet, - tf2::BufferCore & buffer, int fields, ...) + const bool is_dense, const unsigned int scans_per_packet, rclcpp::Clock::SharedPtr clock, + int fields, ...) : config_(min_range, max_range, target_frame, fixed_frame, init_width, init_height, is_dense, scans_per_packet), - tf_buffer_(buffer) + clock_(clock) { va_list vl; cloud.fields.clear(); @@ -92,15 +95,14 @@ class DataContainerBase { double min_range; ///< minimum range to publish double max_range; ///< maximum range to publish - std::string target_frame; ///< target frame to transform a point - std::string fixed_frame; ///< fixed frame used for transform + std::string target_frame; ///< output frame of final point cloud + std::string fixed_frame; ///< world fixed frame for ego motion compenstation unsigned int init_width; unsigned int init_height; bool is_dense; unsigned int scans_per_packet; - bool transform; ///< enable / disable transform points - explicit Config( + Config( double min_range, double max_range, const std::string & target_frame, const std::string & fixed_frame, unsigned int init_width, unsigned int init_height, bool is_dense, unsigned int scans_per_packet) @@ -111,15 +113,17 @@ class DataContainerBase init_width(init_width), init_height(init_height), is_dense(is_dense), - scans_per_packet(scans_per_packet), - transform(fixed_frame != target_frame) + scans_per_packet(scans_per_packet) { } }; - virtual void setup(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg) + virtual void setup(const velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scan_msg) { - cloud.header = scan_msg->header; + sensor_frame_ = scan_msg->header.frame_id; + manage_tf_buffer(); + + cloud.header.stamp = scan_msg->header.stamp; cloud.width = config_.init_width; cloud.height = config_.init_height; cloud.is_dense = static_cast(config_.is_dense); @@ -142,10 +146,42 @@ class DataContainerBase // set to something, then we override that value. if (!config_.target_frame.empty()) { cloud.header.frame_id = config_.target_frame; + } else if (!config_.fixed_frame.empty()) { + cloud.header.frame_id = config_.fixed_frame; + } else { + cloud.header.frame_id = sensor_frame_; } + return cloud; } + void manage_tf_buffer() + { + // check if sensor frame is already known, if not don't prepare tf buffer until setup was called + if (sensor_frame_.empty()) { + return; + } + + // avoid doing transformation when sensor_frame equals target frame + // and no ego motion compensation is perfomed + if (config_.fixed_frame.empty() && sensor_frame_ == config_.target_frame) { + // when the string is empty the points will not be transformed later on + config_.target_frame = ""; + return; + } + + // only use somewhat resource intensive tf listener when transformations are necessary + if (!config_.fixed_frame.empty() || !config_.target_frame.empty()) { + if (!tf_buffer_) { + tf_buffer_ = std::make_shared(clock_); + tf_listener_ = std::make_shared(*tf_buffer_); + } + } else { + tf_listener_.reset(); + tf_buffer_.reset(); + } + } + void configure( const double min_range, const double max_range, const std::string & fixed_frame, const std::string & target_frame) @@ -155,37 +191,60 @@ class DataContainerBase config_.fixed_frame = fixed_frame; config_.target_frame = target_frame; - config_.transform = fixed_frame != target_frame; + manage_tf_buffer(); } - void computeTransformation(const rclcpp::Time & time) + inline bool calculateTransformMatrix( + Eigen::Affine3f & matrix, const std::string & target_frame, + const std::string & source_frame, const rclcpp::Time & time) { - geometry_msgs::msg::TransformStamped transform; + if (!tf_buffer_) { + RCLCPP_ERROR(rclcpp::get_logger("velodyne_pointcloud"), "tf buffer was not initialized yet"); + return false; + } + + geometry_msgs::msg::TransformStamped msg; try { - const std::chrono::nanoseconds dur(time.nanoseconds()); - std::chrono::time_point time(dur); - transform = tf_buffer_.lookupTransform(config_.target_frame, cloud.header.frame_id, time); + msg = tf_buffer_->lookupTransform( + target_frame, source_frame, time, rclcpp::Duration::from_seconds(0.2)); } catch (tf2::LookupException & e) { - return; + RCLCPP_ERROR(rclcpp::get_logger("velodyne_pointcloud"), "%s", e.what()); + return false; } catch (tf2::ExtrapolationException & e) { - return; + RCLCPP_ERROR(rclcpp::get_logger("velodyne_pointcloud"), "%s", e.what()); + return false; + } + + const auto & quaternion = msg.transform.rotation; + Eigen::Quaternionf rotation(quaternion.w, quaternion.x, quaternion.y, quaternion.z); + + const auto & origin = msg.transform.translation; + Eigen::Translation3f translation(origin.x, origin.y, origin.z); + + matrix = translation * rotation; + return true; + } + + inline bool computeTransformToTarget(const rclcpp::Time & scan_time) + { + if (config_.target_frame.empty()) { + // no need to calculate transform -> success + return true; } + std::string & source_frame = config_.fixed_frame.empty() ? sensor_frame_ : config_.fixed_frame; + return calculateTransformMatrix( + tf_matrix_to_target_, config_.target_frame, source_frame, scan_time); + } - tf2::Quaternion quaternion( - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z, - transform.transform.rotation.w); - Eigen::Quaternionf rotation(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); - - Eigen::Vector3f eigen_origin; - tf2::Vector3 origin( - transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z); - vectorTfToEigen(origin, eigen_origin); - Eigen::Translation3f translation(eigen_origin); - transformation = translation * rotation; + inline bool computeTransformToFixed(const rclcpp::Time & packet_time) + { + if (config_.fixed_frame.empty()) { + // no need to calculate transform -> success + return true; + } + std::string & source_frame = sensor_frame_; + return calculateTransformMatrix( + tf_matrix_to_fixed_, config_.fixed_frame, source_frame, packet_time); } protected: @@ -200,7 +259,13 @@ class DataContainerBase inline void transformPoint(float & x, float & y, float & z) { - Eigen::Vector3f p = transformation * Eigen::Vector3f(x, y, z); + Eigen::Vector3f p = Eigen::Vector3f(x, y, z); + if (!config_.fixed_frame.empty()) { + p = tf_matrix_to_fixed_ * p; + } + if (!config_.target_frame.empty()) { + p = tf_matrix_to_target_ * p; + } x = p.x(); y = p.y(); z = p.z(); @@ -212,8 +277,12 @@ class DataContainerBase } Config config_; - tf2::BufferCore & tf_buffer_; - Eigen::Affine3f transformation; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_buffer_; + Eigen::Affine3f tf_matrix_to_fixed_; + Eigen::Affine3f tf_matrix_to_target_; + std::string sensor_frame_; }; } // namespace velodyne_rawdata diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIRT.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIRT.hpp index 524280140..cabd78322 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIRT.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIRT.hpp @@ -48,14 +48,14 @@ class OrganizedCloudXYZIRT final : public velodyne_rawdata::DataContainerBase { public: - explicit OrganizedCloudXYZIRT( + OrganizedCloudXYZIRT( const double min_range, const double max_range, const std::string & target_frame, const std::string & fixed_frame, const unsigned int num_lasers, - const unsigned int scans_per_block, tf2::BufferCore & buffer); + const unsigned int scans_per_block, rclcpp::Clock::SharedPtr clock); void newLine() override; - void setup(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg) override; + void setup(const velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scan_msg) override; void addPoint( float x, float y, float z, const uint16_t ring, diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRT.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRT.hpp index 781cf6fce..735d109f4 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRT.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRT.hpp @@ -48,14 +48,14 @@ class PointcloudXYZIRT final : public velodyne_rawdata::DataContainerBase { public: - explicit PointcloudXYZIRT( + PointcloudXYZIRT( const double min_range, const double max_range, const std::string & target_frame, const std::string & fixed_frame, const unsigned int scans_per_block, - tf2::BufferCore & tf_buffer); + rclcpp::Clock::SharedPtr clock); void newLine() override; - void setup(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg) override; + void setup(const velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scan_msg) override; void addPoint( float x, float y, float z, uint16_t ring, diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp index 57f831fe0..e5c6790b3 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp @@ -63,13 +63,11 @@ class Transform final Transform & operator=(const Transform & c) = delete; private: - void processScan(const std::shared_ptr & scanMsg); + void processScan(const velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scanMsg); std::unique_ptr data_; rclcpp::Publisher::SharedPtr output_; - message_filters::Subscriber velodyne_scan_; - tf2_ros::Buffer tf_buffer_; - std::unique_ptr> tf_filter_; + rclcpp::Subscription::SharedPtr velodyne_scan_; std::unique_ptr container_ptr_; diff --git a/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py b/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py deleted file mode 100644 index ac5ee5e3d..000000000 --- a/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# All rights reserved. -# -# Software License Agreement (BSD License 2.0) -# -# 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 {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 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. - -"""Launch the velodyne pointcloud convert with default configuration.""" - -import os - -import ament_index_python.packages - -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -import yaml - - -def generate_launch_description(): - share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - params_file = os.path.join(share_dir, 'config', 'VLP16-velodyne_convert_node-params.yaml') - with open(params_file, 'r') as f: - params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] - params['calibration'] = os.path.join(share_dir, 'params', 'VLP16db.yaml') - container = ComposableNodeContainer( - name='velodyne_pointcloud_convert_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='velodyne_pointcloud', - plugin='velodyne_pointcloud::Convert', - name='velodyne_convert_node', - parameters=[params]), - ], - output='both', - ) - - return LaunchDescription([container]) diff --git a/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-launch.py b/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-launch.py deleted file mode 100644 index a94c0e616..000000000 --- a/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-launch.py +++ /dev/null @@ -1,63 +0,0 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# All rights reserved. -# -# Software License Agreement (BSD License 2.0) -# -# 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 {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 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. - -"""Launch the velodyne pointcloud convert node with default configuration.""" - -import os - -import ament_index_python.packages -import launch -import launch_ros.actions - -import yaml - - -def generate_launch_description(): - share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - params_file = os.path.join(share_dir, 'config', 'VLP16-velodyne_convert_node-params.yaml') - with open(params_file, 'r') as f: - params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] - params['calibration'] = os.path.join(share_dir, 'params', 'VLP16db.yaml') - velodyne_convert_node = launch_ros.actions.Node(package='velodyne_pointcloud', - executable='velodyne_convert_node', - output='both', - parameters=[params]) - - return launch.LaunchDescription([velodyne_convert_node, - - launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=velodyne_convert_node, - on_exit=[launch.actions.EmitEvent( - event=launch.events.Shutdown())], - )), - ]) diff --git a/velodyne_pointcloud/launch/velodyne_convert_node-VLP32C-composed-launch.py b/velodyne_pointcloud/launch/velodyne_convert_node-VLP32C-composed-launch.py deleted file mode 100644 index 6e838110f..000000000 --- a/velodyne_pointcloud/launch/velodyne_convert_node-VLP32C-composed-launch.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# All rights reserved. -# -# Software License Agreement (BSD License 2.0) -# -# 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 {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 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. - -"""Launch the velodyne pointcloud convert with default configuration.""" - -import os - -import ament_index_python.packages - -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -import yaml - - -def generate_launch_description(): - share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - params_file = os.path.join(share_dir, 'config', 'VLP32C-velodyne_convert_node-params.yaml') - with open(params_file, 'r') as f: - params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] - params['calibration'] = os.path.join(share_dir, 'params', 'VeloView-VLP-32C.yaml') - container = ComposableNodeContainer( - name='velodyne_pointcloud_convert_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='velodyne_pointcloud', - plugin='velodyne_pointcloud::Convert', - name='velodyne_convert_node', - parameters=[params]), - ], - output='both', - ) - - return LaunchDescription([container]) diff --git a/velodyne_pointcloud/launch/velodyne_convert_node-VLP32C-launch.py b/velodyne_pointcloud/launch/velodyne_convert_node-VLP32C-launch.py deleted file mode 100644 index 9e68b5d4d..000000000 --- a/velodyne_pointcloud/launch/velodyne_convert_node-VLP32C-launch.py +++ /dev/null @@ -1,63 +0,0 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# All rights reserved. -# -# Software License Agreement (BSD License 2.0) -# -# 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 {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 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. - -"""Launch the velodyne pointcloud convert node with default configuration.""" - -import os - -import ament_index_python.packages -import launch -import launch_ros.actions - -import yaml - - -def generate_launch_description(): - share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud') - params_file = os.path.join(share_dir, 'config', 'VLP32C-velodyne_convert_node-params.yaml') - with open(params_file, 'r') as f: - params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters'] - params['calibration'] = os.path.join(share_dir, 'params', 'VeloView-VLP-32C.yaml') - velodyne_convert_node = launch_ros.actions.Node(package='velodyne_pointcloud', - executable='velodyne_convert_node', - output='both', - parameters=[params]) - - return launch.LaunchDescription([velodyne_convert_node, - - launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=velodyne_convert_node, - on_exit=[launch.actions.EmitEvent( - event=launch.events.Shutdown())], - )), - ]) diff --git a/velodyne_pointcloud/src/conversions/convert.cpp b/velodyne_pointcloud/src/conversions/convert.cpp deleted file mode 100644 index 74a897623..000000000 --- a/velodyne_pointcloud/src/conversions/convert.cpp +++ /dev/null @@ -1,172 +0,0 @@ -// Copyright 2009, 2010, 2011, 2012, 2019 Austin Robot Technology, Jack O'Quin, Jesse Vera, Joshua Whitley // NOLINT -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// -// 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 {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 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 - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "velodyne_pointcloud/organized_cloudXYZIRT.hpp" -#include "velodyne_pointcloud/pointcloudXYZIRT.hpp" -#include "velodyne_pointcloud/rawdata.hpp" - -namespace velodyne_pointcloud -{ - -/** @brief Constructor. */ -Convert::Convert(const rclcpp::NodeOptions & options) -: rclcpp::Node("velodyne_convert_node", options), - tf_buffer_(this->get_clock()), - diagnostics_(this) -{ - // get path to angles.config file for this device - std::string calibration_file = this->declare_parameter("calibration", ""); - const auto model = this->declare_parameter("model", "64E"); - - rcl_interfaces::msg::ParameterDescriptor min_range_desc; - min_range_desc.name = "min_range"; - min_range_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - min_range_desc.description = "minimum range to publish"; - rcl_interfaces::msg::FloatingPointRange min_range_range; - min_range_range.from_value = 0.1; - min_range_range.to_value = 10.0; - min_range_desc.floating_point_range.push_back(min_range_range); - double min_range = this->declare_parameter("min_range", 0.9, min_range_desc); - - rcl_interfaces::msg::ParameterDescriptor max_range_desc; - max_range_desc.name = "max_range"; - max_range_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - max_range_desc.description = "maximum range to publish"; - rcl_interfaces::msg::FloatingPointRange max_range_range; - max_range_range.from_value = 0.1; - max_range_range.to_value = 200.0; - max_range_desc.floating_point_range.push_back(max_range_range); - double max_range = this->declare_parameter("max_range", 130.0, max_range_desc); - - rcl_interfaces::msg::ParameterDescriptor view_direction_desc; - view_direction_desc.name = "view_direction"; - view_direction_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - view_direction_desc.description = "angle defining the center of view"; - rcl_interfaces::msg::FloatingPointRange view_direction_range; - view_direction_range.from_value = -M_PI; - view_direction_range.to_value = M_PI; - view_direction_desc.floating_point_range.push_back(view_direction_range); - double view_direction = this->declare_parameter("view_direction", 0.0, view_direction_desc); - - rcl_interfaces::msg::ParameterDescriptor view_width_desc; - view_width_desc.name = "view_width"; - view_width_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - view_width_desc.description = "angle defining the view width"; - rcl_interfaces::msg::FloatingPointRange view_width_range; - view_width_range.from_value = 0.0; - view_width_range.to_value = 2.0 * M_PI; - view_width_desc.floating_point_range.push_back(view_width_range); - double view_width = this->declare_parameter("view_width", 2.0 * M_PI, view_width_desc); - - bool organize_cloud = this->declare_parameter("organize_cloud", false); - - std::string target_frame = this->declare_parameter("target_frame", "velodyne"); - std::string fixed_frame = this->declare_parameter("fixed_frame", "velodyne"); - - RCLCPP_INFO(this->get_logger(), "correction angles: %s", calibration_file.c_str()); - - data_ = std::make_unique(calibration_file, model); - - if (organize_cloud) { - container_ptr_ = std::make_unique( - min_range, max_range, target_frame, fixed_frame, - data_->numLasers(), data_->scansPerPacket(), tf_buffer_); - } else { - container_ptr_ = std::make_unique( - min_range, max_range, target_frame, fixed_frame, - data_->scansPerPacket(), tf_buffer_); - } - - // advertise output point cloud (before subscribing to input data) - output_ = - this->create_publisher("velodyne_points", 10); - - // subscribe to VelodyneScan packets - velodyne_scan_ = - this->create_subscription( - "velodyne_packets", rclcpp::QoS(10), - std::bind(&Convert::processScan, this, std::placeholders::_1)); - - // Diagnostics - diagnostics_.setHardwareID("Velodyne Convert"); - // Arbitrary frequencies since we don't know which RPM is used, and are only - // concerned about monitoring the frequency. - diag_min_freq_ = 2.0; - diag_max_freq_ = 20.0; - diag_topic_ = std::make_unique( - "velodyne_points", diagnostics_, - diagnostic_updater::FrequencyStatusParam( - &diag_min_freq_, &diag_max_freq_, 0.1, 10), - diagnostic_updater::TimeStampStatusParam()); - - data_->setParameters(min_range, max_range, view_direction, view_width); - container_ptr_->configure(min_range, max_range, target_frame, fixed_frame); -} - -/** @brief Callback for raw scan messages. */ -void Convert::processScan(const velodyne_msgs::msg::VelodyneScan::SharedPtr scanMsg) -{ - if (output_->get_subscription_count() == 0 && - output_->get_intra_process_subscription_count() == 0) // no one listening? - { - return; // avoid much work - } - - // allocate a point cloud with same time and frame ID as raw data - container_ptr_->setup(scanMsg); - - // process each packet provided by the driver - for (size_t i = 0; i < scanMsg->packets.size(); ++i) { - data_->unpack(scanMsg->packets[i], *container_ptr_, scanMsg->header.stamp); - } - - // publish the accumulated cloud message - diag_topic_->tick(scanMsg->header.stamp); - output_->publish(container_ptr_->finishCloud()); -} - -} // namespace velodyne_pointcloud - -RCLCPP_COMPONENTS_REGISTER_NODE(velodyne_pointcloud::Convert) diff --git a/velodyne_pointcloud/src/conversions/convert_node.cpp b/velodyne_pointcloud/src/conversions/convert_node.cpp deleted file mode 100644 index c4cc3260c..000000000 --- a/velodyne_pointcloud/src/conversions/convert_node.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// -// 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 {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 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 - -#include - -#include "velodyne_pointcloud/convert.hpp" - -/** Main node entry point. */ -int main(int argc, char ** argv) -{ - // Force flush of the stdout buffer. - setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - - rclcpp::init(argc, argv); - - // handle callbacks until shut down - rclcpp::spin( - std::make_shared( - rclcpp::NodeOptions())); - - rclcpp::shutdown(); - - return 0; -} diff --git a/velodyne_pointcloud/src/conversions/transform.cpp b/velodyne_pointcloud/src/conversions/transform.cpp index 1fda6cbf5..2a69706b2 100644 --- a/velodyne_pointcloud/src/conversions/transform.cpp +++ b/velodyne_pointcloud/src/conversions/transform.cpp @@ -53,7 +53,6 @@ namespace velodyne_pointcloud Transform::Transform(const rclcpp::NodeOptions & options) : rclcpp::Node("velodyne_transform_node", options), - velodyne_scan_(this, "velodyne_packets"), tf_buffer_(this->get_clock()), diagnostics_(this) { std::string calibration_file = this->declare_parameter("calibration", ""); @@ -99,8 +98,8 @@ Transform::Transform(const rclcpp::NodeOptions & options) view_width_desc.floating_point_range.push_back(view_width_range); double view_width = this->declare_parameter("view_width", 2.0 * M_PI, view_width_desc); - std::string target_frame = this->declare_parameter("frame_id", "map"); - std::string fixed_frame = this->declare_parameter("fixed_frame", "odom"); + std::string fixed_frame = this->declare_parameter("fixed_frame", ""); + std::string target_frame = this->declare_parameter("target_frame", ""); bool organize_cloud = this->declare_parameter("organize_cloud", true); RCLCPP_INFO(this->get_logger(), "correction angles: %s", calibration_file.c_str()); @@ -110,23 +109,18 @@ Transform::Transform(const rclcpp::NodeOptions & options) if (organize_cloud) { container_ptr_ = std::make_unique( min_range, max_range, target_frame, fixed_frame, data_->numLasers(), - data_->scansPerPacket(), tf_buffer_); + data_->scansPerPacket(), this->get_clock()); } else { container_ptr_ = std::make_unique( min_range, max_range, target_frame, fixed_frame, - data_->scansPerPacket(), tf_buffer_); + data_->scansPerPacket(), this->get_clock()); } // advertise output point cloud (before subscribing to input data) output_ = this->create_publisher("velodyne_points", 10); - // subscribe to VelodyneScan packets using transform filter - tf_filter_ = - std::make_unique>( - velodyne_scan_, tf_buffer_, target_frame, 10, - this->get_node_logging_interface(), this->get_node_clock_interface()); - tf_filter_->registerCallback( - std::bind(&Transform::processScan, this, std::placeholders::_1)); + velodyne_scan_ = this->create_subscription( + "velodyne_packets", 10, std::bind(&Transform::processScan, this, std::placeholders::_1)); // Diagnostics diagnostics_.setHardwareID("Velodyne Transform"); @@ -149,7 +143,7 @@ Transform::Transform(const rclcpp::NodeOptions & options) * the configured @c frame_id can succeed. */ void Transform::processScan( - const std::shared_ptr & scanMsg) + const velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scanMsg) { if (output_->get_subscription_count() == 0 && output_->get_intra_process_subscription_count() == 0) // no one listening? @@ -157,13 +151,22 @@ void Transform::processScan( return; } - velodyne_msgs::msg::VelodyneScan * raw = - const_cast(scanMsg.get()); - container_ptr_->setup(std::shared_ptr(raw)); + container_ptr_->setup(scanMsg); + + // sufficient to calculate single transform for whole scan + if (!container_ptr_->computeTransformToTarget(scanMsg->header.stamp)) { + // target frame not available + return; + } // process each packet provided by the driver for (size_t i = 0; i < scanMsg->packets.size(); ++i) { - container_ptr_->computeTransformation(scanMsg->packets[i].stamp); + // calculate individual transform for each packet to account for ego + // during one rotation of the velodyne sensor + if (!container_ptr_->computeTransformToFixed(scanMsg->packets[i].stamp)) { + // fixed frame not available + return; + } data_->unpack(scanMsg->packets[i], *container_ptr_, scanMsg->header.stamp); } diff --git a/velodyne_pointcloud/src/lib/CMakeLists.txt b/velodyne_pointcloud/src/lib/CMakeLists.txt index a5a7926be..b665594a5 100644 --- a/velodyne_pointcloud/src/lib/CMakeLists.txt +++ b/velodyne_pointcloud/src/lib/CMakeLists.txt @@ -10,6 +10,9 @@ ament_target_dependencies(velodyne_rawdata tf2_ros velodyne_msgs ) +target_link_libraries(velodyne_rawdata + ${PCL_LIBRARIES} +) target_link_libraries(velodyne_rawdata ${YAML_CPP_LIBRARIES}) @@ -21,9 +24,12 @@ add_library(velodyne_cloud_types SHARED ament_target_dependencies(velodyne_cloud_types rclcpp sensor_msgs - tf2 + tf2_ros velodyne_msgs ) +target_link_libraries(velodyne_cloud_types + ${PCL_LIBRARIES} +) install(TARGETS velodyne_cloud_types velodyne_rawdata RUNTIME DESTINATION bin diff --git a/velodyne_pointcloud/src/lib/organized_cloudXYZIRT.cpp b/velodyne_pointcloud/src/lib/organized_cloudXYZIRT.cpp index 759fd913d..89d915e1e 100644 --- a/velodyne_pointcloud/src/lib/organized_cloudXYZIRT.cpp +++ b/velodyne_pointcloud/src/lib/organized_cloudXYZIRT.cpp @@ -47,10 +47,10 @@ OrganizedCloudXYZIRT::OrganizedCloudXYZIRT( const double min_range, const double max_range, const std::string & target_frame, const std::string & fixed_frame, const unsigned int num_lasers, const unsigned int scans_per_block, - tf2::BufferCore & buffer) + rclcpp::Clock::SharedPtr clock) : DataContainerBase( min_range, max_range, target_frame, fixed_frame, - num_lasers, 0, false, scans_per_block, buffer, 6, + num_lasers, 0, false, scans_per_block, clock, 6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, @@ -73,7 +73,7 @@ void OrganizedCloudXYZIRT::newLine() ++cloud.height; } -void OrganizedCloudXYZIRT::setup(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg) +void OrganizedCloudXYZIRT::setup(const velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scan_msg) { DataContainerBase::setup(scan_msg); iter_x_ = sensor_msgs::PointCloud2Iterator(cloud, "x"); @@ -95,9 +95,7 @@ void OrganizedCloudXYZIRT::addPoint( * NaN. */ if (pointInRange(distance)) { - if (config_.transform) { - transformPoint(x, y, z); - } + transformPoint(x, y, z); *(iter_x_ + ring) = x; *(iter_y_ + ring) = y; diff --git a/velodyne_pointcloud/src/lib/pointcloudXYZIRT.cpp b/velodyne_pointcloud/src/lib/pointcloudXYZIRT.cpp index 537ac0ace..16eb3c3a0 100644 --- a/velodyne_pointcloud/src/lib/pointcloudXYZIRT.cpp +++ b/velodyne_pointcloud/src/lib/pointcloudXYZIRT.cpp @@ -45,10 +45,10 @@ namespace velodyne_pointcloud PointcloudXYZIRT::PointcloudXYZIRT( const double min_range, const double max_range, const std::string & target_frame, const std::string & fixed_frame, - const unsigned int scans_per_block, tf2::BufferCore & tf_buffer) + const unsigned int scans_per_block, rclcpp::Clock::SharedPtr clock) : DataContainerBase( min_range, max_range, target_frame, fixed_frame, - 0, 1, true, scans_per_block, tf_buffer, 6, + 0, 1, true, scans_per_block, clock, 6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, @@ -59,7 +59,7 @@ PointcloudXYZIRT::PointcloudXYZIRT( iter_intensity_(cloud, "intensity"), iter_ring_(cloud, "ring"), iter_time_(cloud, "time") {} -void PointcloudXYZIRT::setup(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg) +void PointcloudXYZIRT::setup(const velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scan_msg) { DataContainerBase::setup(scan_msg); iter_x_ = sensor_msgs::PointCloud2Iterator(cloud, "x"); @@ -84,9 +84,7 @@ void PointcloudXYZIRT::addPoint( // convert polar coordinates to Euclidean XYZ - if (config_.transform) { - transformPoint(x, y, z); - } + transformPoint(x, y, z); *iter_x_ = x; *iter_y_ = y; diff --git a/velodyne_pointcloud/tests/CMakeLists.txt b/velodyne_pointcloud/tests/CMakeLists.txt index 624d46dd5..0cdcbb4ac 100644 --- a/velodyne_pointcloud/tests/CMakeLists.txt +++ b/velodyne_pointcloud/tests/CMakeLists.txt @@ -6,7 +6,7 @@ ament_target_dependencies(test_row_step geometry_msgs rclcpp sensor_msgs - tf2 + tf2_ros velodyne_msgs ) @@ -38,17 +38,15 @@ ament_target_dependencies(test_row_step # DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests # MD5 f45c2bb1d7ee358274e423ea3b66fd73) -# # run rostests -# add_rostest(cloud_node_hz.test) -# add_rostest(cloud_nodelet_hz.test) -# add_rostest(cloud_node_32e_hz.test) -# add_rostest(cloud_nodelet_32e_hz.test) -# add_rostest(cloud_node_64e_s2.1_hz.test) -# add_rostest(cloud_nodelet_64e_s2.1_hz.test) -# add_rostest(cloud_node_vlp16_hz.test) -# add_rostest(cloud_nodelet_vlp16_hz.test) +# run rostests # add_rostest(transform_node_hz.test) # add_rostest(transform_nodelet_hz.test) +# add_rostest(transform_node_32e_hz.test) +# add_rostest(transform_nodelet_32e_hz.test) +# add_rostest(transform_node_64e_s2.1_hz.test) +# add_rostest(transform_nodelet_64e_s2.1_hz.test) +# add_rostest(transform_node_vlp16_hz.test) +# add_rostest(transform_nodelet_vlp16_hz.test) # add_rostest(two_nodelet_managers.test) # # parse check all the launch/*.launch files diff --git a/velodyne_pointcloud/tests/cloud_node_hz.test b/velodyne_pointcloud/tests/cloud_node_hz.test deleted file mode 100644 index 834f01fda..000000000 --- a/velodyne_pointcloud/tests/cloud_node_hz.test +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/velodyne_pointcloud/tests/cloud_nodelet_hz.test b/velodyne_pointcloud/tests/cloud_nodelet_hz.test deleted file mode 100644 index e06648c3b..000000000 --- a/velodyne_pointcloud/tests/cloud_nodelet_hz.test +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/velodyne_pointcloud/tests/test_row_step.cpp b/velodyne_pointcloud/tests/test_row_step.cpp index 4741f280e..dc9df9ad8 100644 --- a/velodyne_pointcloud/tests/test_row_step.cpp +++ b/velodyne_pointcloud/tests/test_row_step.cpp @@ -42,10 +42,10 @@ class TestContainer final : public velodyne_rawdata::DataContainerBase { public: - TestContainer(unsigned int width, tf2::BufferCore & buffer) + TestContainer(unsigned int width, rclcpp::Clock::SharedPtr clock) : velodyne_rawdata::DataContainerBase( 0, 0, "target", "fixed", width, 0, false, 0, - buffer, 1, "x", 1, sensor_msgs::msg::PointField::FLOAT32) + clock, 1, "x", 1, sensor_msgs::msg::PointField::FLOAT32) { } @@ -84,8 +84,8 @@ class TestContainer final TEST(datacontainerbase, row_step_zero_width_constructor) { - tf2::BufferCore core; - TestContainer cont(0, core); + auto clock = std::make_shared(); + TestContainer cont(0, clock); ASSERT_EQ(cont.getCloudWidth(), 0U); ASSERT_EQ(cont.getCloudPointStep(), 4U); ASSERT_EQ(cont.getCloudRowStep(), 0U); @@ -93,8 +93,8 @@ TEST(datacontainerbase, row_step_zero_width_constructor) TEST(datacontainerbase, row_step_one_width_constructor) { - tf2::BufferCore core; - TestContainer cont(1, core); + auto clock = std::make_shared(); + TestContainer cont(1, clock); ASSERT_EQ(cont.getCloudWidth(), 1U); ASSERT_EQ(cont.getCloudPointStep(), 4U); ASSERT_EQ(cont.getCloudRowStep(), 4U); @@ -102,8 +102,8 @@ TEST(datacontainerbase, row_step_one_width_constructor) TEST(datacontainerbase, row_step_one_width_after_setup) { - tf2::BufferCore core; - TestContainer cont(1, core); + auto clock = std::make_shared(); + TestContainer cont(1, clock); auto msg = std::make_shared(); cont.setup(msg); ASSERT_EQ(cont.getCloudWidth(), 1U); diff --git a/velodyne_pointcloud/tests/cloud_node_32e_hz.test b/velodyne_pointcloud/tests/transform_node_32e_hz.test similarity index 59% rename from velodyne_pointcloud/tests/cloud_node_32e_hz.test rename to velodyne_pointcloud/tests/transform_node_32e_hz.test index 2844b89ce..bafb8a726 100644 --- a/velodyne_pointcloud/tests/cloud_node_32e_hz.test +++ b/velodyne_pointcloud/tests/transform_node_32e_hz.test @@ -9,20 +9,20 @@ - - + + - + - - - + + + diff --git a/velodyne_pointcloud/tests/cloud_node_64e_s2.1_hz.test b/velodyne_pointcloud/tests/transform_node_64e_s2.1_hz.test similarity index 61% rename from velodyne_pointcloud/tests/cloud_node_64e_s2.1_hz.test rename to velodyne_pointcloud/tests/transform_node_64e_s2.1_hz.test index c3ab1c6ea..088cfb6bc 100644 --- a/velodyne_pointcloud/tests/cloud_node_64e_s2.1_hz.test +++ b/velodyne_pointcloud/tests/transform_node_64e_s2.1_hz.test @@ -10,20 +10,20 @@ - - + + - + - - - + + + diff --git a/velodyne_pointcloud/tests/transform_node_hz.test b/velodyne_pointcloud/tests/transform_node_hz.test index 795b934ad..631121f8b 100644 --- a/velodyne_pointcloud/tests/transform_node_hz.test +++ b/velodyne_pointcloud/tests/transform_node_hz.test @@ -13,8 +13,9 @@ - + + + - + + - + - - - + + + diff --git a/velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test b/velodyne_pointcloud/tests/transform_nodelet_32e_hz.test similarity index 57% rename from velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test rename to velodyne_pointcloud/tests/transform_nodelet_32e_hz.test index 3f28ee127..f10661b55 100644 --- a/velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test +++ b/velodyne_pointcloud/tests/transform_nodelet_32e_hz.test @@ -3,20 +3,20 @@ - + - + - - - + + + diff --git a/velodyne_pointcloud/tests/cloud_nodelet_64e_s2.1_hz.test b/velodyne_pointcloud/tests/transform_nodelet_64e_s2.1_hz.test similarity index 61% rename from velodyne_pointcloud/tests/cloud_nodelet_64e_s2.1_hz.test rename to velodyne_pointcloud/tests/transform_nodelet_64e_s2.1_hz.test index 9470ea60d..63dbc1a98 100644 --- a/velodyne_pointcloud/tests/cloud_nodelet_64e_s2.1_hz.test +++ b/velodyne_pointcloud/tests/transform_nodelet_64e_s2.1_hz.test @@ -10,20 +10,20 @@ - - + + - + - - - + + + diff --git a/velodyne_pointcloud/tests/transform_nodelet_hz.test b/velodyne_pointcloud/tests/transform_nodelet_hz.test index d7107bf41..14d7a6b5e 100644 --- a/velodyne_pointcloud/tests/transform_nodelet_hz.test +++ b/velodyne_pointcloud/tests/transform_nodelet_hz.test @@ -13,8 +13,9 @@ - + + + + - + - - - + + + diff --git a/velodyne_pointcloud/tests/two_nodelet_managers.test b/velodyne_pointcloud/tests/two_nodelet_managers.test index 03b7c4183..3218dd8ca 100644 --- a/velodyne_pointcloud/tests/two_nodelet_managers.test +++ b/velodyne_pointcloud/tests/two_nodelet_managers.test @@ -7,7 +7,7 @@ - + @@ -27,7 +27,7 @@ - +