Merge pull request #214 from spuetz/feature/opc_nopcl

Container cleanup and organized pointclouds

velodyne_driver/launch/nodelet_manager.launch

@@ -17,7 +17,7 @@
   <arg name="cut_angle" default="-0.01" />
 
   <!-- start nodelet manager -->
-  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" />
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
 
   <!-- load driver nodelet into it -->
   <node pkg="nodelet" type="nodelet" name="$(arg manager)_driver"

velodyne_driver/package.xml

@@ -25,7 +25,6 @@
   <depend>dynamic_reconfigure</depend>
   <depend>libpcap</depend>
   <depend>nodelet</depend>
-  <depend>pluginlib</depend>
   <depend>roscpp</depend>
   <depend>tf</depend>
   <depend>velodyne_msgs</depend>

velodyne_pointcloud/CMakeLists.txt

@@ -11,25 +11,24 @@ elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98")
 endif()
 
 set(${PROJECT_NAME}_CATKIN_DEPS
-  angles
-  nodelet
-  pcl_ros
-  roscpp
-  roslib
-  sensor_msgs
-  tf
-  velodyne_driver
-  velodyne_msgs
-  dynamic_reconfigure
-  diagnostic_updater
+    angles
+    nodelet
+    roscpp
+    roslib
+    sensor_msgs
+    tf
+    velodyne_driver
+    velodyne_msgs
+    dynamic_reconfigure
+    diagnostic_updater
 )
 
 find_package(catkin REQUIRED COMPONENTS
              ${${PROJECT_NAME}_CATKIN_DEPS}
-             pcl_conversions
              roslint)
-find_package(Boost COMPONENTS signals)
 
+find_package(Boost COMPONENTS signals)
+find_package(Eigen3 REQUIRED)
 # Resolve system dependency on yaml-cpp, which apparently does not
 # provide a CMake find_package() module.
 find_package(PkgConfig REQUIRED)
@@ -53,6 +52,7 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
 
 include_directories(include ${catkin_INCLUDE_DIRS}
   ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake
+  ${EIGEN3_INCLUDE_DIR}
 )
 
 catkin_package(

velodyne_pointcloud/cfg/CloudNode.cfg

@@ -12,5 +12,6 @@ gen.add("view_direction", double_t, 0, "angle defining the center of view",
         0.0, -pi, pi)
 gen.add("view_width", double_t, 0, "angle defining the view width",
         2*pi, 0.0, 2*pi)
+gen.add("organize_cloud", bool_t, 0, "organized cloud", False)
 
 exit(gen.generate(PACKAGE, "cloud_node", "CloudNode"))

velodyne_pointcloud/cfg/TransformNode.cfg

@@ -36,4 +36,10 @@ gen.add("frame_id",
   "fixed frame of reference for point clouds",
   "map")
 
+gen.add("organize_cloud",
+  pgc.bool_t,
+  0,
+  "organize cloud",
+  False)
+
 exit(gen.generate(PACKAGE, "transform_node", "TransformNode"))

velodyne_pointcloud/include/velodyne_pointcloud/convert.h

@@ -39,13 +39,14 @@
 #ifndef VELODYNE_POINTCLOUD_CONVERT_H
 #define VELODYNE_POINTCLOUD_CONVERT_H
 
+#include <string>
+
 #include <ros/ros.h>
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/publisher.h>
 
 #include <sensor_msgs/PointCloud2.h>
 #include <velodyne_pointcloud/rawdata.h>
-#include <velodyne_pointcloud/pointcloudXYZIR.h>
 
 #include <dynamic_reconfigure/server.h>
 #include <velodyne_pointcloud/CloudNodeConfig.h>
@@ -54,31 +55,38 @@ namespace velodyne_pointcloud
 {
 class Convert
 {
-public:
-  Convert(ros::NodeHandle node, ros::NodeHandle private_nh);
-  ~Convert() {}
-
-private:
-  void callback(
-    velodyne_pointcloud::CloudNodeConfig &config,
-    uint32_t level);
-  void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg);
-
-  // Pointer to dynamic reconfigure service srv_
-  boost::shared_ptr<dynamic_reconfigure::Server<velodyne_pointcloud::
-    CloudNodeConfig> > srv_;
-
-  boost::shared_ptr<velodyne_rawdata::RawData> data_;
-  ros::Subscriber velodyne_scan_;
-  ros::Publisher output_;
-
-  /// configuration parameters
-  typedef struct
-  {
-    int npackets;                    // number of packets to combine
-  }
-  Config;
-  Config config_;
+  public:
+    Convert(ros::NodeHandle node, ros::NodeHandle private_nh);
+    ~Convert() {}
+
+  private:
+    void callback(velodyne_pointcloud::CloudNodeConfig &config, uint32_t level);
+    void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg);
+
+    boost::shared_ptr<dynamic_reconfigure::Server<velodyne_pointcloud::CloudNodeConfig> > srv_;
+
+    boost::shared_ptr<velodyne_rawdata::RawData> data_;
+    ros::Subscriber velodyne_scan_;
+    ros::Publisher output_;
+
+    boost::shared_ptr<velodyne_rawdata::DataContainerBase> container_ptr_;
+
+    boost::mutex reconfigure_mtx_;
+
+    /// configuration parameters
+    typedef struct
+    {
+      std::string target_frame;      ///< target frame
+      std::string fixed_frame;       ///< fixed frame
+      bool organize_cloud;           ///< enable/disable organized cloud structure
+      double max_range;              ///< maximum range to publish
+      double min_range;              ///< minimum range to publish
+      uint16_t num_lasers;           ///< number of lasers
+      int npackets;                  ///< number of packets to combine
+    }
+    Config;
+    Config config_;
+    bool first_rcfg_call;
 
   // diagnostics updater
   diagnostic_updater::Updater diagnostics_;

velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h

@@ -1,4 +1,6 @@
-// Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley
+#ifndef VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
+#define VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
+// Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz
 // All rights reserved.
 //
 // Software License Agreement (BSD License 2.0)
@@ -30,25 +32,178 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#ifndef VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
-#define VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
-
-#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <velodyne_msgs/VelodyneScan.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <Eigen/Dense>
+#include <string>
+#include <algorithm>
+#include <cstdarg>
 
 namespace velodyne_rawdata
 {
 class DataContainerBase
 {
 public:
-  virtual void addPoint(
-    const float& x,
-    const float& y,
-    const float& z,
-    const uint16_t& ring,
-    const uint16_t& azimuth,
-    const float& distance,
-    const float& intensity) = 0;
-};
-}  // namespace velodyne_rawdata
+  DataContainerBase(const double max_range, const double min_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,
+                    boost::shared_ptr<tf::TransformListener>& tf_ptr, int fields, ...)
+    : config_(max_range, min_range, target_frame, fixed_frame, init_width, init_height, is_dense, scans_per_packet)
+    , tf_ptr(tf_ptr)
+  {
+    va_list vl;
+    cloud.fields.clear();
+    cloud.fields.reserve(fields);
+    va_start(vl, fields);
+    int offset = 0;
+    for (int i = 0; i < fields; ++i)
+    {
+      // Create the corresponding PointField
+      std::string name(va_arg(vl, char*));
+      int count(va_arg(vl, int));
+      int datatype(va_arg(vl, int));
+      offset = addPointField(cloud, name, count, datatype, offset);
+    }
+    va_end(vl);
+    cloud.point_step = offset;
+    cloud.row_step = cloud.width * cloud.point_step;
+    if (config_.transform && !tf_ptr)
+    {
+      tf_ptr = boost::shared_ptr<tf::TransformListener>(new tf::TransformListener);
+    }
+  }
+
+  struct Config
+  {
+    double max_range;          ///< maximum range to publish
+    double min_range;          ///< minimum range to publish
+    std::string target_frame;  ///< target frame to transform a point
+    std::string fixed_frame;   ///< fixed frame used for transform
+    unsigned int init_width;
+    unsigned int init_height;
+    bool is_dense;
+    unsigned int scans_per_packet;
+    bool transform;  ///< enable / disable transform points
+
+    Config(double max_range, double min_range, std::string target_frame, std::string fixed_frame,
+           unsigned int init_width, unsigned int init_height, bool is_dense, unsigned int scans_per_packet)
+      : max_range(max_range)
+      , min_range(min_range)
+      , target_frame(target_frame)
+      , fixed_frame(fixed_frame)
+      , transform(fixed_frame != target_frame)
+      , init_width(init_width)
+      , init_height(init_height)
+      , is_dense(is_dense)
+      , scans_per_packet(scans_per_packet)
+    {
+      ROS_INFO_STREAM("Initialized container with "
+                      << "min_range: " << min_range << ", max_range: " << max_range
+                      << ", target_frame: " << target_frame << ", fixed_frame: " << fixed_frame
+                      << ", init_with: " << init_width << ", init_height: " << init_height << ", is_dense: " << is_dense
+                      << ", scans_per_packet: " << scans_per_packet);
+    }
+  };
+
+  virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg)
+  {
+    cloud.header = scan_msg->header;
+    cloud.data.resize(scan_msg->packets.size() * config_.scans_per_packet * cloud.point_step);
+    cloud.width = config_.init_width;
+    cloud.height = config_.init_height;
+    cloud.is_dense = static_cast<uint8_t>(config_.is_dense);
+    if (config_.transform)
+    {
+      if (!computeTransformation(scan_msg->header.stamp))
+      {
+        ROS_ERROR_STREAM("Could not transform points!");
+      }
+    }
+  }
+
+  virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance,
+                        const float intensity) = 0;
+  virtual void newLine() = 0;
+
+  const sensor_msgs::PointCloud2& finishCloud()
+  {
+    cloud.data.resize(cloud.point_step * cloud.width * cloud.height);
+    cloud.header.frame_id = config_.target_frame;
+    ROS_DEBUG_STREAM("Prepared cloud width" << cloud.height * cloud.width
+                                            << " Velodyne points, time: " << cloud.header.stamp);
+    return cloud;
+  }
 
+  void configure(const double max_range, const double min_range, const std::string fixed_frame,
+                 const std::string target_frame)
+  {
+    config_.max_range = max_range;
+    config_.min_range = min_range;
+    config_.fixed_frame = fixed_frame;
+    config_.target_frame = target_frame;
+
+    config_.transform = fixed_frame.compare(target_frame) != 0;
+    if (config_.transform && !tf_ptr)
+    {
+      tf_ptr = boost::shared_ptr<tf::TransformListener>(new tf::TransformListener);
+    }
+  }
+
+  sensor_msgs::PointCloud2 cloud;
+
+protected:
+  inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3f& eigen_vec)
+  {
+    eigen_vec(0) = tf_vec[0];
+    eigen_vec(1) = tf_vec[1];
+    eigen_vec(2) = tf_vec[2];
+  }
+
+  inline bool computeTransformation(const ros::Time& time)
+  {
+    tf::StampedTransform transform;
+    try
+    {
+      tf_ptr->lookupTransform(config_.target_frame, cloud.header.frame_id, time, transform);
+    }
+    catch (tf::LookupException& e)
+    {
+      ROS_ERROR("%s", e.what());
+      return false;
+    }
+    catch (tf::ExtrapolationException& e)
+    {
+      ROS_ERROR("%s", e.what());
+      return false;
+    }
+
+    tf::Quaternion quaternion = transform.getRotation();
+    Eigen::Quaternionf rotation(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
+
+    Eigen::Vector3f eigen_origin;
+    vectorTfToEigen(transform.getOrigin(), eigen_origin);
+    Eigen::Translation3f translation(eigen_origin);
+    transformation = translation * rotation;
+    return true;
+  }
+
+  inline void transformPoint(float& x, float& y, float& z)
+  {
+    Eigen::Vector3f p = transformation * Eigen::Vector3f(x, y, z);
+    x = p.x();
+    y = p.y();
+    z = p.z();
+  }
+
+  inline bool pointInRange(float range)
+  {
+    return (range >= config_.min_range && range <= config_.max_range);
+  }
+
+  Config config_;
+  boost::shared_ptr<tf::TransformListener> tf_ptr;  ///< transform listener
+  Eigen::Affine3f transformation;
+};
+} /* namespace velodyne_rawdata */
 #endif  // VELODYNE_POINTCLOUD_DATACONTAINERBASE_H