Initial pointcloud to laserscan commit

pointcloud_to_laserscan/CMakeLists.txt

@@ -0,0 +1,94 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pointcloud_to_laserscan)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
+  message_filters
+  nodelet
+  pcl_ros
+  roscpp
+  sensor_msgs
+)
+find_package(PCL REQUIRED)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES pointcloud_to_laserscan
+#  CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs
+#  DEPENDS system_lib
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(PointCloudToLaserScan src/PointCloudToLaserScanBase.cpp)
+#add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg)
+target_link_libraries(PointCloudToLaserScan ${catkin_LIBRARIES})
+
+add_executable(pointcloud_to_laserscan src/pointcloud_to_laserscan_node.cpp)
+#add_dependencies(pointcloud_to_laserscan pointcloud_to_laserscan_generate_messages_cpp)
+target_link_libraries(pointcloud_to_laserscan PointCloudToLaserScan ${catkin_LIBRARIES})
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_pointcloud_to_laserscan.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h

@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * 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 the 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: Chad Rockey
+ */
+
+#ifndef POINTCLOUD_TO_LASERSCAN_ROS
+#define POINTCLOUD_TO_LASERSCAN_ROS
+
+#include <ros/ros.h>
+#include <boost/thread/mutex.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/ros/conversions.h>
+
+#include <tf/transform_listener.h>
+
+namespace pointcloud_to_laserscan
+{ 
+typedef pcl::PointXYZ Point;
+typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+
+
+class PointCloudToLaserScanBase
+{
+public:
+    
+
+    PointCloudToLaserScanBase(ros::NodeHandle& n, ros::NodeHandle& pnh, const unsigned int concurrency);
+    
+    ~PointCloudToLaserScanBase();
+
+private:
+
+    void cloudCb(const PointCloud::ConstPtr& cloud);
+
+    void connectCb();
+
+    void disconnectCb();
+    
+    ros::NodeHandle nh_;
+    ros::NodeHandle private_nh_;
+
+    ros::Publisher pub_;
+    ros::Subscriber sub_;
+
+    tf::TransformListener tf_;
+
+    boost::mutex connect_mutex_;
+    const unsigned int concurrency_;
+
+    std::string target_frame_;
+    float tolerance_;
+};
+
+
+} // pointcloud_to_laserscan
+
+#endif

pointcloud_to_laserscan/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package>
+  <name>pointcloud_to_laserscan</name>
+  <version>0.0.0</version>
+  <description>The pointcloud_to_laserscan package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="paul@todo.todo">paul</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/pointcloud_to_laserscan</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>libpcl-all-dev</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp

@@ -0,0 +1,44 @@
+#include <pointcloud_to_laserscan/PointCloudToLaserScanBase.h>
+#include <sensor_msgs/LaserScan.h>
+
+
+namespace pointcloud_to_laserscan
+{ 
+
+PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh, const unsigned int concurrency) :
+    nh_(nh), private_nh_(private_nh), concurrency_(concurrency)
+{
+    boost::mutex::scoped_lock lock(connect_mutex_);
+    pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, boost::bind(&PointCloudToLaserScanBase::connectCb, this), boost::bind(&PointCloudToLaserScanBase::disconnectCb, this));
+
+    //todo params
+    target_frame_ = "base_link";
+
+}
+
+PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){
+    sub_.shutdown();
+}
+
+void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud){
+
+}
+
+
+void PointCloudToLaserScanBase::connectCb() {
+    boost::mutex::scoped_lock lock(connect_mutex_);
+    if (!sub_ && pub_.getNumSubscribers() > 0) {
+        ROS_DEBUG("Connecting to depth topic.");
+        sub_ = nh_.subscribe("cloud_in", concurrency_, &PointCloudToLaserScanBase::cloudCb, this);
+    }
+}
+
+void PointCloudToLaserScanBase::disconnectCb() {
+    boost::mutex::scoped_lock lock(connect_mutex_);
+    if (pub_.getNumSubscribers() == 0) {
+        ROS_DEBUG("Unsubscribing from depth topic.");
+        sub_.shutdown();
+    }
+}
+
+} // pointcloud_to_laserscan

pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp

@@ -0,0 +1,16 @@
+#include <ros/ros.h>
+#include <pointcloud_to_laserscan/PointCloudToLaserScanBase.h>
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "pointcloud_to_laserscan");
+    ros::NodeHandle nh;
+    ros::NodeHandle nh_private("~");
+
+    unsigned int concurrency = 4;
+
+    pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, nh_private, concurrency);
+
+    ros::spin();
+
+    return 0;
+}