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 @@
-
+