new: LaserScan to PointCloud node + nodelet

CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(pointcloud_to_laserscan)
 
 find_package(catkin REQUIRED COMPONENTS
+  laser_geometry
   message_filters
   nodelet
   roscpp
@@ -13,22 +14,32 @@ find_package(catkin REQUIRED COMPONENTS
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES pointcloud_to_laserscan
-  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros
+  LIBRARIES laserscan_to_pointcloud pointcloud_to_laserscan
+  CATKIN_DEPENDS laser_geometry message_filters nodelet roscpp sensor_msgs tf2 tf2_ros tf2_sensor_msgs
 )
 
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
 )
 
+add_library(laserscan_to_pointcloud src/laserscan_to_pointcloud_nodelet.cpp)
+target_link_libraries(laserscan_to_pointcloud ${catkin_LIBRARIES})
+
+add_executable(laserscan_to_pointcloud_node src/laserscan_to_pointcloud_node.cpp)
+target_link_libraries(laserscan_to_pointcloud_node laserscan_to_pointcloud ${catkin_LIBRARIES})
+
 add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp)
 target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
 
 add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp)
 target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES})
 
-install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node
+install(TARGETS
+  laserscan_to_pointcloud
+  laserscan_to_pointcloud_node
+  pointcloud_to_laserscan
+  pointcloud_to_laserscan_node
 	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

include/pointcloud_to_laserscan/laserscan_to_pointcloud_nodelet.h

@@ -0,0 +1,96 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Eurotec, Netherlands
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+/*
+ * Author: Rein Appeldoorn
+ */
+
+#ifndef POINTCLOUD_TO_LASERSCAN_LASERSCAN_TO_POINTCLOUD_NODELET_H
+#define POINTCLOUD_TO_LASERSCAN_LASERSCAN_TO_POINTCLOUD_NODELET_H
+
+#include <boost/thread/mutex.hpp>
+#include <laser_geometry/laser_geometry.h>
+#include <message_filters/subscriber.h>
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <string>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace pointcloud_to_laserscan
+{
+typedef tf2_ros::MessageFilter<sensor_msgs::LaserScan> MessageFilter;
+
+//! \brief The PointCloudToLaserScanNodelet class to process incoming laserscans into pointclouds.
+//!
+class LaserScanToPointCloudNodelet : public nodelet::Nodelet
+{
+public:
+  LaserScanToPointCloudNodelet();
+
+private:
+  virtual void onInit();
+
+  void scanCallback(const sensor_msgs::LaserScanConstPtr& scan_msg);
+  void failureCallback(const sensor_msgs::LaserScanConstPtr& scan_msg,
+                       tf2_ros::filter_failure_reasons::FilterFailureReason reason);
+
+  void connectCb();
+  void disconnectCb();
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+  ros::Publisher pub_;
+  boost::mutex connect_mutex_;
+
+  boost::shared_ptr<tf2_ros::Buffer> tf2_;
+  boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+  message_filters::Subscriber<sensor_msgs::LaserScan> sub_;
+  boost::shared_ptr<MessageFilter> message_filter_;
+
+  laser_geometry::LaserProjection projector_;
+
+  // ROS Parameters
+  unsigned int input_queue_size_;
+  std::string target_frame_;
+  double transform_tolerance_;
+};
+
+}  // namespace pointcloud_to_laserscan
+
+#endif  // POINTCLOUD_TO_LASERSCAN_LASERSCAN_TO_POINTCLOUD_NODELET_H

launch/sample_laserscan_to_pointcloud_node.launch

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+
+<launch>
+
+    <arg name="scanner" default="scanner" />
+
+    <!-- Publish as laserscan-->
+    <node pkg="rostopic" type="rostopic" args="pub $(arg scanner)/scan sensor_msgs/LaserScan -r 10
+      '{header: {frame_id: scan},
+        angle_min: -1.0, angle_max: 1.0, angle_increment: 0.1,
+        range_max: 10.0,
+        ranges: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]}'" name="scan_publisher" />
+
+    <!-- run laserscan_to_pointcloud node -->
+    <node pkg="pointcloud_to_laserscan" type="laserscan_to_pointcloud_node" name="laserscan_to_pointcloud">
+
+        <remap from="scan_in" to="$(arg scanner)/scan"/>
+        <remap from="cloud" to="$(arg scanner)/cloud"/>
+        <rosparam>
+            target_frame: scan # Leave disabled to output pointcloud in scan frame
+            transform_tolerance: 0.01
+
+            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
+            # 0 : Detect number of cores
+            # 1 : Single threaded
+            # 2->inf : Parallelism level
+            concurrency_level: 1
+        </rosparam>
+
+    </node>
+
+</launch>

launch/sample_laserscan_to_pointcloud_nodelet.launch

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+
+<launch>
+
+    <arg name="scanner" default="scanner" />
+
+    <!-- Publish as laserscan-->
+    <node pkg="rostopic" type="rostopic" args="pub $(arg scanner)/scan sensor_msgs/LaserScan -r 10
+      '{header: {frame_id: scan},
+        angle_min: -1.0, angle_max: 1.0, angle_increment: 0.1,
+        range_max: 10.0,
+        ranges: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]}'" name="scan_publisher" />
+
+    <!-- run nodelet manager -->
+    <node pkg="nodelet" type="nodelet" name="$(arg scanner)_nodelet_manager" args="manager" />
+
+    <!-- run laserscan_to_pointcloud nodelet -->
+    <node pkg="nodelet" type="nodelet" name="laserscan_to_pointcloud" args="load pointcloud_to_laserscan/laserscan_to_pointcloud_nodelet $(arg scanner)_nodelet_manager">
+
+        <remap from="scan_in" to="$(arg scanner)/scan"/>
+        <remap from="cloud" to="$(arg scanner)/cloud"/>
+        <rosparam>
+            target_frame: scan # Leave disabled to output pointcloud in scan frame
+            transform_tolerance: 0.01
+
+            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
+            # 0 : Detect number of cores
+            # 1 : Single threaded
+            # 2->inf : Parallelism level
+            concurrency_level: 1
+        </rosparam>
+
+    </node>
+
+</launch>

nodelets.xml

@@ -9,3 +9,15 @@
   </class>
 
 </library>
+
+<library path="lib/liblaserscan_to_pointcloud">
+
+  <class name="pointcloud_to_laserscan/laserscan_to_pointcloud_nodelet"
+   type="pointcloud_to_laserscan::LaserScanToPointCloudNodelet"
+   base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet to convert sensor_msgs/LaserScan to sensor_msgs/PointCloud2.
+    </description>
+  </class>
+
+</library>

package.xml

@@ -16,6 +16,7 @@
 
     <buildtool_depend>catkin</buildtool_depend>
 
+    <depend>laser_geometry</depend>
     <depend>message_filters</depend>
     <depend>nodelet</depend>
     <depend>roscpp</depend>

src/laserscan_to_pointcloud_node.cpp

@@ -0,0 +1,68 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Eurotec, Netherlands
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+/*
+ * Author: Rein Appeldoorn
+ */
+
+#include <ros/ros.h>
+#include <nodelet/loader.h>
+#include <string>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "laserscan_to_pointcloud_node");
+  ros::NodeHandle private_nh("~");
+  int concurrency_level = private_nh.param("concurrency_level", 0);
+
+  nodelet::Loader nodelet;
+  nodelet::M_string remap(ros::names::getRemappings());
+  nodelet::V_string nargv;
+  std::string nodelet_name = ros::this_node::getName();
+  nodelet.load(nodelet_name, "pointcloud_to_laserscan/laserscan_to_pointcloud_nodelet", remap, nargv);
+
+  boost::shared_ptr<ros::MultiThreadedSpinner> spinner;
+  if (concurrency_level)
+  {
+    spinner.reset(new ros::MultiThreadedSpinner(static_cast<uint32_t>(concurrency_level)));
+  }
+  else
+  {
+    spinner.reset(new ros::MultiThreadedSpinner());
+  }
+  spinner->spin();
+  return 0;
+}