Merge pull request #17 from pf-telos/fix/laserscan-metadata

Fix LaserScan metadata

docs/metadata.md

@@ -0,0 +1,88 @@
+## Scan metadata
+
+The calculation of metadata in LaserScan and PointCloud2 messages such as timestamps and
+resolution information has been reworked in `bugfix/laserscan-metadata` branch.
+
+The following `sensor_msgs/msg/LaserScan` fields are affected:
+
+                                 # in frame frame_id, angles are measured around
+                                 # the positive Z axis (counterclockwise, if Z is up)
+                                 # with zero angle being forward along the x axis
+
+    float32 angle_min            # start angle of the scan [rad]
+    float32 angle_max            # end angle of the scan [rad]
+    float32 angle_increment      # angular distance between measurements [rad]
+
+    float32 time_increment       # time between measurements [seconds] - if your scanner
+                                 # is moving, this will be used in interpolating position
+                                 # of 3d points
+    float32 scan_time            # time between scans [seconds]
+
+    float32 range_min            # minimum range value [m]
+    float32 range_max            # maximum range value [m]
+
+### Changes
+
+#### `scan_time`
+
+`scan_time` previously always was set to zero due to a data type mismatch in
+code.  It is now derived from the `scan_frequency` as reported by the sensor in
+the packet header. It tells the *configured* scan frequency (PFSDP parameter
+`scan_frequency`), not the actual physical speed (`scan_frequency_measured`).
+
+For a 4-layer R2300 the driver distributes `LaserScan` messages on four
+separate topics, one per each layer, so that subscribers can differentiate
+between layers (there is no elevation information in `LaserScan`). In this
+configuration, each `scan_time` is calculated per layer, thus appears to be 
+four times longer than for 1-layer R2300.
+
+#### `angle_min`, `angle_max`, `angle_increment`
+
+`angle_min` specifies the angle at which the very first measurement of the
+`LaserScan` was taken, as reported in the header of the first packet of a scan.
+
+Previously it told the *configured* start angle. Consequently, the `angle_max`,
+which was previously computed from configured and dynamic information, is now
+set from data received in the packet headers:
+
+    angle_max := angle_min + (num_points_scan+1) * angle_increment
+
+Note that a scanner which is configured to rotate *clockwise*, which differs
+from the default (`ccw`) behaviour, outputs negative `angle_increment` which
+might come unexpected:
+
+    angle_increment < 0
+
+    angle_max < angle_min
+
+Decimation caused by optional scan data filtering is taken into account. A
+`filter_width=4` would cause a proportionally larger `angle_increment`. 
+
+#### `time_increment` 
+
+`time_increment` previously was derived from `scan_time` and thus
+always zero (see above) but now will give reasonable information.
+
+For R2000 it is still computed from the `scan_time`, which is computed the
+from *configured* `scan_frequency` value, simply by dividing:
+
+    R2000 time_increment := scan_time / (360° / angular_increment)
+
+If rotational speed is slightly off, especially when target speed was just
+reconfigured or or due to external physical impacts, also the `time_increment`
+also might be a little off the truth.
+
+The R2300 sample rate is not as tightly coupled to the current speed as in
+R2000. For both `scan_frequency` options, the nominal sample rate is 90 kHz.
+
+    R2300 time_increment := 1/90 kHz
+
+Again the decimation caused by optional scan data filtering is taken into account.
+If four actual measurements are averaged into one point, the `time_increment` is
+accordingly four times longer than without filterung.
+
+Note that `time_increment` in many situations is *not* 
+
+    time_increment NOT= scan_time / (angle_max - angle_min)
+
+

src/pf_driver/include/pf_driver/pf/pfsdp_base.h

@@ -122,8 +122,6 @@ class PFSDPBase
 
   virtual std::string get_product();
 
-  virtual std::string get_part();
-
   virtual void get_scan_parameters();
 
   void setup_parameters_callback();

src/pf_driver/include/pf_driver/pf/r2000/pfsdp_2000.h

@@ -10,8 +10,6 @@ class PFSDP_2000 : public PFSDPBase
   PFSDP_2000(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<HandleInfo> info, std::shared_ptr<ScanConfig> config,
              std::shared_ptr<ScanParameters> params);
 
-  virtual std::string get_product();
-
   virtual void get_scan_parameters();
 
   virtual void declare_specific_parameters() override;

src/pf_driver/include/pf_driver/pf/r2300/pfsdp_2300.h

@@ -8,23 +8,13 @@ class PFSDP_2300 : public PFSDPBase
   PFSDP_2300(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<HandleInfo> info, std::shared_ptr<ScanConfig> config,
              std::shared_ptr<ScanParameters> params);
 
-  virtual std::string get_product();
-
-  virtual std::string get_part();
-
   virtual void get_scan_parameters();
 
   void setup_param_server();
 
 private:
   std::shared_ptr<rclcpp::Node> node_;
 
-  void get_layers_enabled(uint16_t& enabled, uint16_t& highest);
-
-  virtual std::pair<float, float> get_angle_start_stop(int start_angle);
-
-  virtual std::string get_start_angle_str();
-
   virtual bool reconfig_callback_impl(const std::vector<rclcpp::Parameter>& parameters) override;
 
   virtual void declare_specific_parameters() override;

src/pf_driver/include/pf_driver/pf/scan_parameters.h

@@ -3,15 +3,11 @@
 #pragma pack(push, sp, 1)
 struct ScanParameters
 {
-  double angular_fov = 0.0;
   double radial_range_min = 0.0;
   double radial_range_max = 0.0;
-  double angle_min = 0.0;
-  double angle_max = 0.0;
-  uint16_t layers_enabled = 0;
-  double scan_freq = 0.0;        // needed to calculate scan resolution in R2300
-  uint16_t h_enabled_layer = 0;  // highest enabled layer
   bool apply_correction = true;
+  int sampling_rate_max = 252000;
+  int scan_time_factor = 1;
 
   // void print()
   // {

src/pf_driver/include/pf_driver/ros/point_cloud_publisher.h

@@ -14,7 +14,7 @@ class PointcloudPublisher : public PFDataPublisher
 public:
   PointcloudPublisher(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<ScanConfig> config,
                       std::shared_ptr<ScanParameters> params, const std::string& scan_topic,
-                      const std::string& frame_id, const uint16_t num_layers, const std::string& part);
+                      const std::string& frame_id, const uint16_t num_layers);
 
 private:
   std::shared_ptr<rclcpp::Node> node_;

src/pf_driver/src/pf/pf_interface.cpp

@@ -241,6 +241,7 @@ bool PFInterface::handle_version(int major_version, int minor_version, int devic
   {
     expected_dev = "R2000";
     protocol_interface_ = std::make_shared<PFSDP_2000>(node_, info_, config_, params_);
+    params_->scan_time_factor = 1;
     reader_ = std::shared_ptr<PFPacketReader>(
         new LaserscanPublisher(node_, config_, params_, topic.c_str(), frame_id.c_str()));
   }
@@ -251,12 +252,13 @@ bool PFInterface::handle_version(int major_version, int minor_version, int devic
 
     if (device_family == 5)
     {
-      std::string part = protocol_interface_->get_part();
+      params_->scan_time_factor = num_layers;
       reader_ = std::shared_ptr<PFPacketReader>(
-          new PointcloudPublisher(node_, config_, params_, topic.c_str(), frame_id.c_str(), num_layers, part.c_str()));
+          new PointcloudPublisher(node_, config_, params_, topic.c_str(), frame_id.c_str(), num_layers));
     }
     else if (device_family == 7)
     {
+      params_->scan_time_factor = 1;
       reader_ = std::shared_ptr<PFPacketReader>(
           new LaserscanPublisher(node_, config_, params_, topic.c_str(), frame_id.c_str()));
     }

src/pf_driver/src/pf/pfsdp_base.cpp

@@ -367,12 +367,7 @@ bool PFSDPBase::feed_watchdog(const std::string& handle)
 
 std::string PFSDPBase::get_product()
 {
-  return std::string("");
-}
-
-std::string PFSDPBase::get_part()
-{
-  return std::string("");
+  return get_parameter_str("product");
 }
 
 void PFSDPBase::get_scan_parameters()

src/pf_driver/src/pf/r2000/pfsdp_2000.cpp

@@ -17,21 +17,12 @@ PFSDP_2000::PFSDP_2000(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<Handl
       node_->add_on_set_parameters_callback(std::bind(&PFSDP_2000::reconfig_callback, this, std::placeholders::_1));
 }
 
-std::string PFSDP_2000::get_product()
-{
-  return get_parameter_str("product");
-}
-
 void PFSDP_2000::get_scan_parameters()
 {
-  auto resp = get_parameter("angular_fov", "radial_range_min", "radial_range_max", "scan_frequency");
-  params_->angular_fov = parser_utils::to_float(resp["angular_fov"]) * M_PI / 180.0;
+  auto resp = get_parameter("radial_range_min", "radial_range_max", "sampling_rate_max");
   params_->radial_range_max = parser_utils::to_float(resp["radial_range_max"]);
   params_->radial_range_min = parser_utils::to_float(resp["radial_range_min"]);
-
-  params_->angle_min = config_->start_angle / 10000.0f * M_PI / 180.0;
-  params_->angle_max = params_->angle_min + params_->angular_fov;
-  params_->scan_freq = parser_utils::to_float(resp["scan_frequency"]);
+  params_->sampling_rate_max = parser_utils::to_long(resp["sampling_rate_max"]);
 }
 
 void PFSDP_2000::declare_specific_parameters()

src/pf_driver/src/pf/r2300/pfsdp_2300.cpp

@@ -18,61 +18,12 @@ PFSDP_2300::PFSDP_2300(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<Handl
       node_->add_on_set_parameters_callback(std::bind(&PFSDP_2300::reconfig_callback, this, std::placeholders::_1));
 }
 
-std::string PFSDP_2300::get_product()
-{
-  return get_parameter_str("product");
-}
-
-std::string PFSDP_2300::get_part()
-{
-  return get_parameter_str("product");
-}
-
 void PFSDP_2300::get_scan_parameters()
 {
-  auto resp = get_parameter("angular_fov", "radial_range_min", "radial_range_max", "measure_start_angle",
-                            "measure_stop_angle", "scan_frequency");
-  params_->angular_fov = parser_utils::to_float(resp["angular_fov"]) * M_PI / 180.0;
+  auto resp = get_parameter("radial_range_min", "radial_range_max", "sampling_rate_max");
   params_->radial_range_max = parser_utils::to_float(resp["radial_range_max"]);
   params_->radial_range_min = parser_utils::to_float(resp["radial_range_min"]);
-
-  auto start_stop = get_angle_start_stop(config_->start_angle);
-  params_->angle_min = start_stop.first;
-  params_->angle_max = start_stop.second;
-  get_layers_enabled(params_->layers_enabled, params_->h_enabled_layer);
-  params_->scan_freq = parser_utils::to_float(resp["scan_frequency"]);
-}
-
-void PFSDP_2300::get_layers_enabled(uint16_t& enabled, uint16_t& highest)
-{
-  enabled = 0;
-  std::string layers = get_parameter_str("layer_enable");
-  std::vector<std::string> vals = parser_utils::split(layers);
-  std::vector<bool> enabled_layers(vals.size(), false);
-  for (int i = 0; i < vals.size(); i++)
-  {
-    if (vals[i].compare("on") == 0)
-    {
-      enabled += std::pow(2, i);
-      highest = i;
-    }
-  }
-}
-
-std::pair<float, float> PFSDP_2300::get_angle_start_stop(int start_angle)
-{
-  float measure_start_angle = get_parameter_float("measure_start_angle") / 10000.0 * M_PI / 180.0;
-  float measure_stop_angle = get_parameter_float("measure_stop_angle") / 10000.0 * M_PI / 180.0;
-  start_angle = start_angle * M_PI / 180.0;
-
-  // float min = (measure_start_angle > start_angle) ? measure_start_angle : start_angle;
-  // float max = measure_stop_angle;
-  return std::pair<float, float>(measure_start_angle, measure_stop_angle);
-}
-
-std::string PFSDP_2300::get_start_angle_str()
-{
-  return std::string("start_angle");
+  params_->sampling_rate_max = parser_utils::to_long(resp["sampling_rate_max"]);
 }
 
 void PFSDP_2300::declare_specific_parameters()

src/pf_driver/src/ros/pf_data_publisher.cpp

@@ -1,13 +1,16 @@
 // #include <exception>
 // #include <limits>
 // #include <utility>
+#include <rclcpp/rclcpp.hpp>
 #include <rclcpp/duration.hpp>
 #include "pf_driver/ros/pf_data_publisher.h"
 #include "pf_driver/pf/pf_packet/pf_r2000_packet_a.h"
 #include "pf_driver/pf/pf_packet/pf_r2000_packet_b.h"
 #include "pf_driver/pf/pf_packet/pf_r2000_packet_c.h"
 #include "pf_driver/pf/pf_packet/pf_r2300_packet_c1.h"
 
+#include <cmath>
+
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
 #endif
@@ -67,7 +70,7 @@ void PFDataPublisher::to_msg_queue(T& packet, uint16_t layer_idx, int layer_incl
     d_queue_.pop_front();
   if (packet.header.header.packet_number == 1)
   {
-    const auto scan_time = rclcpp::Duration(1000.0 / packet.header.scan_frequency, 0);
+    const auto scan_time = rclcpp::Duration(0, 1000000ul * (1000000ul / packet.header.scan_frequency));
     msg.reset(new sensor_msgs::msg::LaserScan());
     msg->header.frame_id.assign(frame_id_);
     // msg->header.seq = packet.header.header.scan_number;
@@ -76,24 +79,36 @@ void PFDataPublisher::to_msg_queue(T& packet, uint16_t layer_idx, int layer_incl
     msg->angle_increment = packet.header.angular_increment / 10000.0 * (M_PI / 180.0);
 
     {
-      msg->time_increment = (params_->angular_fov * msg->scan_time) / (M_PI * 2.0) / packet.header.num_points_scan;
-      msg->angle_min = params_->angle_min;
-      msg->angle_max = params_->angle_max;
-      if (std::is_same<T, PFR2300Packet_C1>::value)  // Only Packet C1 for R2300
+      /* Assuming that angle_min always means *first* angle (which may be numerically
+       * greater than angle_max in case of negative angular_increment during CW rotation) */
+
+      msg->angle_min = ((double)packet.header.first_angle) * (M_PI / 1800000.0);
+      msg->angle_max = msg->angle_min +
+                       ((double)packet.header.num_points_scan * packet.header.angular_increment) * (M_PI / 1800000.0);
+
+      if (std::is_same<T, PFR2300Packet_C1>::value)  // packet interpretation specific to R2300 output
       {
-        double config_start_angle = config_->start_angle / 1800000.0 * M_PI;
-        if (config_start_angle > params_->angle_min)
+        /* If scans are output on separate topics per layer, the time between messages per topic grows by the number of
+         * layers */
+        if (params_->scan_time_factor > 1)
         {
-          msg->angle_min = config_start_angle;
+          msg->scan_time *= (float)(params_->scan_time_factor);
         }
-        if (config_->max_num_points_scan != 0)  // means need to calculate
+
+        double orig_angular_increment = 0.1 * (M_PI / 180.0);
+        if (packet.header.scan_frequency > 50000)
         {
-          double config_angle = (config_->max_num_points_scan - 1) * (params_->scan_freq / 500.0) / 180.0 * M_PI;
-          if (msg->angle_min + config_angle < msg->angle_max)
-          {
-            msg->angle_max = msg->angle_min + config_angle;
-          }
-        }
+          orig_angular_increment = 0.2 * (M_PI / 180.0);
+        };
+        /* Consider effective longer time_increment due to filtering with decimation */
+        double decimation = round(orig_angular_increment / msg->angle_increment);
+
+        /* Assuming that sampling_rate_max==sampling_rate_min==const. */
+        msg->time_increment = decimation / (double)(params_->sampling_rate_max);
+      }
+      else
+      {
+        msg->time_increment = fabs(scan_time.seconds() * (double)packet.header.angular_increment * (1.0 / 3600000.0));
       }
 
       msg->range_min = params_->radial_range_min;

src/pf_driver/src/ros/point_cloud_publisher.cpp

@@ -8,8 +8,7 @@
 
 PointcloudPublisher::PointcloudPublisher(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<ScanConfig> config,
                                          std::shared_ptr<ScanParameters> params, const std::string& scan_topic,
-                                         const std::string& frame_id, const uint16_t num_layers,
-                                         const std::string& part)
+                                         const std::string& frame_id, const uint16_t num_layers)
   : PFDataPublisher(config, params), node_(node), layer_prev_(-1)
 {
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());