- added parameter for publishing and subscription topic

- make convert node actually do transformation of points if target and fixed frame are different
- bugfix: configuration mixed up target and fixed frame

velodyne_pointcloud/include/velodyne_pointcloud/convert.hpp

@@ -37,9 +37,11 @@
 #include <diagnostic_updater/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <tf2_ros/buffer.h>
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
 #include <memory>
 #include <string>
 
@@ -60,21 +62,15 @@ class Convert final
   Convert & operator=(const Convert & c) = delete;
 
 private:
-  void processScan(const velodyne_msgs::msg::VelodyneScan::SharedPtr scanMsg);
+  void processScan(velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scanMsg);
 
   std::unique_ptr<velodyne_rawdata::RawData> data_;
   rclcpp::Subscription<velodyne_msgs::msg::VelodyneScan>::SharedPtr velodyne_scan_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr output_;
   tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
   std::unique_ptr<velodyne_rawdata::DataContainerBase> 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_;

velodyne_pointcloud/src/conversions/convert.cpp

@@ -43,6 +43,8 @@
 #include <memory>
 #include <string>
 
+#include "tf2_ros/create_timer_ros.h"
+
 #include "velodyne_pointcloud/organized_cloudXYZIRT.hpp"
 #include "velodyne_pointcloud/pointcloudXYZIRT.hpp"
 #include "velodyne_pointcloud/rawdata.hpp"
@@ -54,6 +56,7 @@ namespace velodyne_pointcloud
 Convert::Convert(const rclcpp::NodeOptions & options)
 : rclcpp::Node("velodyne_convert_node", options),
   tf_buffer_(this->get_clock()),
+  tf_listener_(tf_buffer_),
   diagnostics_(this)
 {
   // get path to angles.config file for this device
@@ -100,10 +103,12 @@ Convert::Convert(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);
 
-  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");
+  bool organize_cloud = this->declare_parameter("organize_cloud", false);
+
+  std::string topic_in = this->declare_parameter("subscrition_topic", "velodyne_packets");
+  std::string topic_out = this->declare_parameter("publishing_topic", "velodyne_points");
 
   RCLCPP_INFO(this->get_logger(), "correction angles: %s", calibration_file.c_str());
 
@@ -121,12 +126,12 @@ Convert::Convert(const rclcpp::NodeOptions & options)
 
   // advertise output point cloud (before subscribing to input data)
   output_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("velodyne_points", rclcpp::SensorDataQoS());
+    this->create_publisher<sensor_msgs::msg::PointCloud2>(topic_out, rclcpp::SensorDataQoS());
 
   // subscribe to VelodyneScan packets
   velodyne_scan_ =
     this->create_subscription<velodyne_msgs::msg::VelodyneScan>(
-    "velodyne_packets", rclcpp::SensorDataQoS(),
+    topic_in, rclcpp::SensorDataQoS(),
     std::bind(&Convert::processScan, this, std::placeholders::_1));
 
   // Diagnostics
@@ -136,17 +141,17 @@ Convert::Convert(const rclcpp::NodeOptions & options)
   diag_min_freq_ = 2.0;
   diag_max_freq_ = 20.0;
   diag_topic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
-    "velodyne_points", diagnostics_,
-    diagnostic_updater::FrequencyStatusParam(
+    topic_out, 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);
+  container_ptr_->configure(min_range, max_range, fixed_frame, target_frame);
 }
 
 /** @brief Callback for raw scan messages. */
-void Convert::processScan(const velodyne_msgs::msg::VelodyneScan::SharedPtr scanMsg)
+void Convert::processScan(velodyne_msgs::msg::VelodyneScan::ConstSharedPtr scanMsg)
 {
   if (output_->get_subscription_count() == 0 &&
     output_->get_intra_process_subscription_count() == 0)   // no one listening?
@@ -159,6 +164,7 @@ void Convert::processScan(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan
 
   // process each packet provided by the driver
   for (size_t i = 0; i < scanMsg->packets.size(); ++i) {
+    container_ptr_->computeTransformation(scanMsg->packets[i].stamp);
     data_->unpack(scanMsg->packets[i], *container_ptr_, scanMsg->header.stamp);
   }