Merge pull request ros-drivers#110 from kmhallen/master

Added velodyne_laserscan package

velodyne/package.xml

@@ -18,6 +18,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <run_depend>velodyne_driver</run_depend>
+  <run_depend>velodyne_laserscan</run_depend>
   <run_depend>velodyne_msgs</run_depend>
   <run_depend>velodyne_pointcloud</run_depend>
 

velodyne_laserscan/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(velodyne_laserscan)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  nodelet
+  sensor_msgs
+  dynamic_reconfigure
+)
+
+generate_dynamic_reconfigure_options(
+  cfg/VelodyneLaserScan.cfg
+)
+
+catkin_package()
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/VelodyneLaserScan.cpp
+  src/nodelet.cpp
+)
+add_dependencies(${PROJECT_NAME}
+  ${PROJECT_NAME}_gencfg
+)
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_node
+  src/node.cpp
+)
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+  ${PROJECT_NAME}
+)
+
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(FILES nodelets.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+if (CATKIN_ENABLE_TESTING)
+  add_subdirectory(tests)
+endif()
+

velodyne_laserscan/cfg/VelodyneLaserScan.cfg

@@ -0,0 +1,13 @@
+#! /usr/bin/env python
+
+PACKAGE='velodyne_laserscan'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                    Type      Lvl   Description                                     Default   Min     Max
+gen.add("ring",                 int_t,    0,    "Ring to extract as laser scan (-1 default)",   -1,       -1,     31)
+gen.add("resolution",           double_t, 0,    "Laser scan angular resolution (rad)",          0.007,    0.001,  0.05)
+
+exit(gen.generate(PACKAGE, PACKAGE, "VelodyneLaserScan"))

velodyne_laserscan/nodelets.xml

@@ -0,0 +1,10 @@
+<library path="libvelodyne_laserscan">
+  <class name="velodyne_laserscan/LaserScanNodelet"
+         type="velodyne_laserscan::LaserScanNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Extract a single ring from a Velodyne PointCloud2 and publish
+      as a LaserScan.
+    </description>
+  </class>
+</library>

velodyne_laserscan/package.xml

@@ -0,0 +1,31 @@
+<package format="2">
+
+  <name>velodyne_laserscan</name>
+  <version>1.2.0</version>
+  <description>
+    Extract a single ring of a Velodyne PointCloud2 and publish it as a LaserScan message
+  </description>
+  <maintainer email="jack.oquin@gmail.com">Jack O'Quin</maintainer>
+  <author>Micho Radovnikovich</author>
+  <author>Kevin Hallenbeck</author>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/velodyne_laserscan</url>
+  <url type="repository">https://github.com/ros-drivers/velodyne</url>
+  <url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>nodelet</depend>
+  <depend>sensor_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
+
+  <test_depend>roslaunch</test_depend>
+  <test_depend>rostest</test_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelets.xml" />
+  </export>
+
+</package>

velodyne_laserscan/src/VelodyneLaserScan.cpp

@@ -0,0 +1,174 @@
+#include "VelodyneLaserScan.h"
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace velodyne_laserscan {
+
+VelodyneLaserScan::VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv) :
+    ring_count_(0), nh_(nh), srv_(nh_priv)
+{
+  ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this);
+  pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, connect_cb, connect_cb);
+
+  srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));
+}
+
+void VelodyneLaserScan::connectCb()
+{
+  boost::lock_guard<boost::mutex> lock(connect_mutex_);
+  if (!pub_.getNumSubscribers()) {
+    sub_.shutdown();
+  } else if (!sub_) {
+    sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this);
+  }
+}
+
+void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
+{
+  // Latch ring count
+  if (!ring_count_) {
+    // Check for PointCloud2 field 'ring'
+    bool found = false;
+    for (size_t i = 0; i < msg->fields.size(); i++) {
+      if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16) {
+        if (msg->fields[i].name == "ring") {
+          found = true;
+          break;
+        }
+      }
+    }
+    if (!found) {
+      ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
+      return;
+    }
+    for (sensor_msgs::PointCloud2ConstIterator<uint16_t> it(*msg, "ring"); it != it.end(); ++it) {
+      const uint16_t ring = *it;
+      if (ring + 1 > ring_count_) {
+        ring_count_ = ring + 1;
+      }
+    }
+    if (ring_count_) {
+      ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_);
+    } else {
+      ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
+      return;
+    }
+  }
+
+  // Select ring to use
+  uint16_t ring;
+  if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_)) {
+    // Default to ring closest to being level for each known sensor
+    if (ring_count_ > 32) {
+      ring = 57; // HDL-64E
+    } else if (ring_count_ > 16) {
+      ring = 23; // HDL-32E
+    } else {
+      ring = 8; // VLP-16
+    }
+  } else {
+    ring = cfg_.ring;
+  }
+  ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring);
+
+  // Load structure of PointCloud2
+  int offset_x = -1;
+  int offset_y = -1;
+  int offset_z = -1;
+  int offset_i = -1;
+  int offset_r = -1;
+  for (size_t i = 0; i < msg->fields.size(); i++) {
+    if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32) {
+      if (msg->fields[i].name == "x") {
+        offset_x = msg->fields[i].offset;
+      } else if (msg->fields[i].name == "y") {
+        offset_y = msg->fields[i].offset;
+      } else if (msg->fields[i].name == "z") {
+        offset_z = msg->fields[i].offset;
+      } else if (msg->fields[i].name == "intensity") {
+        offset_i = msg->fields[i].offset;
+      }
+    } else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16) {
+      if (msg->fields[i].name == "ring") {
+        offset_r = msg->fields[i].offset;
+      }
+    }
+  }
+
+  // Construct LaserScan message
+  if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0)) {
+    const float RESOLUTION = fabsf(cfg_.resolution);
+    const size_t SIZE = 2.0 * M_PI / RESOLUTION;
+    sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
+    scan->header = msg->header;
+    scan->angle_increment = RESOLUTION;
+    scan->angle_min = -M_PI;
+    scan->angle_max = M_PI;
+    scan->range_min = 0.0;
+    scan->range_max = 200.0;
+    scan->time_increment = 0.0;
+    scan->ranges.resize(SIZE, INFINITY);
+    if ((offset_x == 0) && (offset_y == 4) && (offset_z == 8) && (offset_i == 16) && (offset_r == 20)) {
+      scan->intensities.resize(SIZE);
+      for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it) {
+        const uint16_t r = *((const uint16_t*)(&it[5])); // ring
+        if (r == ring) {
+          const float x = it[0]; // x
+          const float y = it[1]; // y
+          const float i = it[4]; // intensity
+          const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION;
+          if ((bin >= 0) && (bin < (int)SIZE)) {
+            scan->ranges[bin] = sqrtf(x * x + y * y);
+            scan->intensities[bin] = i;
+          }
+        }
+      }
+    } else {
+      ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method.");
+      if (offset_i >= 0) {
+        scan->intensities.resize(SIZE);
+        sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_i(*msg, "intensity");
+        for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i) {
+          const uint16_t r = *iter_r; // ring
+          if (r == ring) {
+            const float x = *iter_x; // x
+            const float y = *iter_y; // y
+            const float i = *iter_i; // intensity
+            const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION;
+            if ((bin >= 0) && (bin < (int)SIZE)) {
+              scan->ranges[bin] = sqrtf(x * x + y * y);
+              scan->intensities[bin] = i;
+            }
+          }
+        }
+      } else {
+        sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+        for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r) {
+          const uint16_t r = *iter_r; // ring
+          if (r == ring) {
+            const float x = *iter_x; // x
+            const float y = *iter_y; // y
+            const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION;
+            if ((bin >= 0) && (bin < (int)SIZE)) {
+              scan->ranges[bin] = sqrtf(x * x + y * y);
+            }
+          }
+        }
+      }
+    }
+    pub_.publish(scan);
+  } else {
+    ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)");
+  }
+}
+
+void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level)
+{
+  cfg_ = config;
+}
+
+}

velodyne_laserscan/src/VelodyneLaserScan.h

@@ -0,0 +1,39 @@
+#ifndef VELODYNELASERSCAN_H
+#define VELODYNELASERSCAN_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/lock_guard.hpp>
+
+#include <dynamic_reconfigure/server.h>
+#include <velodyne_laserscan/VelodyneLaserScanConfig.h>
+
+namespace velodyne_laserscan {
+
+class VelodyneLaserScan
+{
+public:
+  VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv);
+
+private:
+  boost::mutex connect_mutex_;
+  void connectCb();
+  void recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
+
+  ros::NodeHandle nh_;
+  ros::Subscriber sub_;
+  ros::Publisher pub_;
+
+  VelodyneLaserScanConfig cfg_;
+  dynamic_reconfigure::Server<VelodyneLaserScanConfig> srv_;
+  void reconfig(VelodyneLaserScanConfig& config, uint32_t level);
+
+  unsigned int ring_count_;
+};
+
+}
+
+#endif // VELODYNELASERSCAN_H

velodyne_laserscan/src/node.cpp

@@ -0,0 +1,17 @@
+#include <ros/ros.h>
+#include "VelodyneLaserScan.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "velodyne_laserscan_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
+
+  // create VelodyneLaserScan class
+  velodyne_laserscan::VelodyneLaserScan n(nh, nh_priv);
+
+  // handle callbacks until shut down
+  ros::spin();
+
+  return 0;
+}

velodyne_laserscan/src/nodelet.cpp

@@ -0,0 +1,24 @@
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+#include "VelodyneLaserScan.h"
+
+namespace velodyne_laserscan
+{
+
+class LaserScanNodelet: public nodelet::Nodelet
+{
+public:
+  LaserScanNodelet() {}
+  ~LaserScanNodelet() {}
+
+private:
+  virtual void onInit() {
+    node_.reset(new VelodyneLaserScan(getNodeHandle(), getPrivateNodeHandle()));
+  }
+  boost::shared_ptr<VelodyneLaserScan> node_;
+};
+
+}
+
+PLUGINLIB_DECLARE_CLASS(velodyne_laserscan, LaserScanNodelet, velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);