From 85241bcb537b44339c0d4efc4171344115f43648 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sat, 20 Sep 2014 16:30:19 -0400 Subject: [PATCH] Initial pointcloud to laserscan commit --- pointcloud_to_laserscan/CMakeLists.txt | 94 +++++++++++++++++++ .../PointCloudToLaserScanBase.h | 88 +++++++++++++++++ pointcloud_to_laserscan/package.xml | 67 +++++++++++++ .../src/PointCloudToLaserScanBase.cpp | 44 +++++++++ .../src/PointCloudToLaserScanNodelet.cpp | 0 .../src/pointcloud_to_laserscan_node.cpp | 16 ++++ 6 files changed, 309 insertions(+) create mode 100644 pointcloud_to_laserscan/CMakeLists.txt create mode 100644 pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h create mode 100644 pointcloud_to_laserscan/package.xml create mode 100644 pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp create mode 100644 pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp create mode 100644 pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt new file mode 100644 index 00000000..6018a529 --- /dev/null +++ b/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) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h new file mode 100644 index 00000000..9bc12ffd --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include + +#include + +namespace pointcloud_to_laserscan +{ +typedef pcl::PointXYZ Point; +typedef pcl::PointCloud 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 diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml new file mode 100644 index 00000000..228a606c --- /dev/null +++ b/pointcloud_to_laserscan/package.xml @@ -0,0 +1,67 @@ + + + pointcloud_to_laserscan + 0.0.0 + The pointcloud_to_laserscan package + + + + + paul + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + dynamic_reconfigure + libpcl-all-dev + message_filters + nodelet + pcl_ros + roscpp + sensor_msgs + dynamic_reconfigure + libpcl-all-dev + message_filters + nodelet + pcl_ros + roscpp + sensor_msgs + + + + + + + + + + + \ No newline at end of file diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp new file mode 100644 index 00000000..3374c9f1 --- /dev/null +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -0,0 +1,44 @@ +#include +#include + + +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("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 diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp new file mode 100644 index 00000000..e69de29b diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp new file mode 100644 index 00000000..8c56f448 --- /dev/null +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp @@ -0,0 +1,16 @@ +#include +#include + +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; +}