Remove pointcloud_to_laserscan package
This commit is contained in:
Paul Bovbel
Paul Bovbel
parent
8ffb7a3189
commit
5472ff8f8a
@ -23,7 +23,6 @@
|
|||||||
<run_depend>pcl_conversions</run_depend>
|
<run_depend>pcl_conversions</run_depend>
|
||||||
<run_depend>pcl_msgs</run_depend>
|
<run_depend>pcl_msgs</run_depend>
|
||||||
<run_depend>pcl_ros</run_depend>
|
<run_depend>pcl_ros</run_depend>
|
||||||
<run_depend>pointcloud_to_laserscan</run_depend>
|
|
||||||
<export>
|
<export>
|
||||||
<metapackage/>
|
<metapackage/>
|
||||||
</export>
|
</export>
|
||||||
|
|||||||
@ -1,43 +0,0 @@
|
|||||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
|
||||||
Changelog for package pointcloud_to_laserscan
|
|
||||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
|
||||||
|
|
||||||
1.2.6 (2015-02-04)
|
|
||||||
------------------
|
|
||||||
* Fix default value for concurrency
|
|
||||||
* Fix multithreaded lazy pub sub
|
|
||||||
* Contributors: Paul Bovbel
|
|
||||||
|
|
||||||
1.2.5 (2015-01-20)
|
|
||||||
------------------
|
|
||||||
* Switch to tf_sensor_msgs for transform
|
|
||||||
* Set parameters in sample launch files to default
|
|
||||||
* Add tolerance parameter
|
|
||||||
* Contributors: Paul Bovbel
|
|
||||||
|
|
||||||
1.2.4 (2015-01-15)
|
|
||||||
------------------
|
|
||||||
* Remove stray dependencies
|
|
||||||
* Refactor with tf2 and message filters
|
|
||||||
* Remove roslaunch check
|
|
||||||
* Fix regressions
|
|
||||||
* Refactor to allow debug messages from node and nodelet
|
|
||||||
* Contributors: Paul Bovbel
|
|
||||||
|
|
||||||
1.2.3 (2015-01-10)
|
|
||||||
------------------
|
|
||||||
* add launch tests
|
|
||||||
* refactor naming and fix nodelet export
|
|
||||||
* set default target frame to empty
|
|
||||||
* clean up package.xml
|
|
||||||
* Contributors: Paul Bovbel
|
|
||||||
|
|
||||||
1.2.2 (2014-10-25)
|
|
||||||
------------------
|
|
||||||
* clean up package.xml
|
|
||||||
* Fix header reference
|
|
||||||
* Fix flow
|
|
||||||
* Fix pointer assertion
|
|
||||||
* Finalize pointcloud to laserscan
|
|
||||||
* Initial pointcloud to laserscan commit
|
|
||||||
* Contributors: Paul Bovbel
|
|
||||||
@ -1,45 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 2.8.3)
|
|
||||||
project(pointcloud_to_laserscan)
|
|
||||||
|
|
||||||
find_package(catkin REQUIRED COMPONENTS
|
|
||||||
message_filters
|
|
||||||
nodelet
|
|
||||||
roscpp
|
|
||||||
sensor_msgs
|
|
||||||
tf2
|
|
||||||
tf2_ros
|
|
||||||
tf2_sensor_msgs
|
|
||||||
)
|
|
||||||
|
|
||||||
catkin_package(
|
|
||||||
INCLUDE_DIRS include
|
|
||||||
LIBRARIES pointcloud_to_laserscan
|
|
||||||
CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros
|
|
||||||
)
|
|
||||||
|
|
||||||
include_directories(
|
|
||||||
include
|
|
||||||
${catkin_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
|
|
||||||
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
|
|
||||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
|
||||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
|
||||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
|
||||||
|
|
||||||
install(DIRECTORY include/${PROJECT_NAME}/
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
|
||||||
)
|
|
||||||
install(FILES nodelets.xml
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY launch
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
)
|
|
||||||
@ -1,96 +0,0 @@
|
|||||||
/*
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2010-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 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: Paul Bovbel
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
|
|
||||||
#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
|
|
||||||
|
|
||||||
#include "ros/ros.h"
|
|
||||||
#include "boost/thread/mutex.hpp"
|
|
||||||
|
|
||||||
#include "nodelet/nodelet.h"
|
|
||||||
#include "tf2_ros/buffer.h"
|
|
||||||
#include "tf2_ros/message_filter.h"
|
|
||||||
#include "message_filters/subscriber.h"
|
|
||||||
#include "sensor_msgs/PointCloud2.h"
|
|
||||||
|
|
||||||
|
|
||||||
namespace pointcloud_to_laserscan
|
|
||||||
{
|
|
||||||
typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
|
|
||||||
/**
|
|
||||||
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
|
|
||||||
* pointcloud_to_laserscan implementation.
|
|
||||||
*/
|
|
||||||
class PointCloudToLaserScanNodelet : public nodelet::Nodelet
|
|
||||||
{
|
|
||||||
|
|
||||||
public:
|
|
||||||
PointCloudToLaserScanNodelet();
|
|
||||||
|
|
||||||
private:
|
|
||||||
virtual void onInit();
|
|
||||||
|
|
||||||
void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
|
|
||||||
void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
|
|
||||||
tf2_ros::filter_failure_reasons::FilterFailureReason reason);
|
|
||||||
|
|
||||||
void connectCb();
|
|
||||||
|
|
||||||
void disconnectCb();
|
|
||||||
|
|
||||||
ros::NodeHandle nh_, private_nh_;
|
|
||||||
ros::Publisher pub_;
|
|
||||||
boost::mutex connect_mutex_;
|
|
||||||
|
|
||||||
tf2_ros::Buffer tf2_;
|
|
||||||
message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
|
|
||||||
boost::shared_ptr<MessageFilter> message_filter_;
|
|
||||||
|
|
||||||
// ROS Parameters
|
|
||||||
unsigned int input_queue_size_;
|
|
||||||
std::string target_frame_;
|
|
||||||
double tolerance_;
|
|
||||||
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
|
|
||||||
bool use_inf_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // pointcloud_to_laserscan
|
|
||||||
|
|
||||||
#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
|
|
||||||
@ -1,40 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
|
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="camera" default="camera" />
|
|
||||||
|
|
||||||
<!-- start sensor-->
|
|
||||||
<include file="$(find openni2_launch)/launch/openni2.launch">
|
|
||||||
<arg name="camera" default="$(arg camera)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!-- run pointcloud_to_laserscan node -->
|
|
||||||
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
|
|
||||||
|
|
||||||
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
|
|
||||||
<remap from="scan" to="$(arg camera)/scan"/>
|
|
||||||
<rosparam>
|
|
||||||
#target_frame: # Leave disabled to output scan in pointcloud frame
|
|
||||||
transform_tolerance: 0.01
|
|
||||||
min_height: 0.0
|
|
||||||
max_height: 1.0
|
|
||||||
|
|
||||||
angle_min: -1.5708 # -M_PI/2
|
|
||||||
angle_max: 1.5708 # M_PI/2
|
|
||||||
angle_increment: 0.087 # M_PI/360.0
|
|
||||||
scan_time: 0.3333
|
|
||||||
range_min: 0.45
|
|
||||||
range_max: 4.0
|
|
||||||
use_inf: true
|
|
||||||
|
|
||||||
# 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: 0
|
|
||||||
</rosparam>
|
|
||||||
|
|
||||||
</node>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
@ -1,40 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
|
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="camera" default="camera" />
|
|
||||||
|
|
||||||
<!-- start sensor-->
|
|
||||||
<include file="$(find openni2_launch)/launch/openni2.launch">
|
|
||||||
<arg name="camera" default="$(arg camera)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!-- push pointcloud_to_laserscan nodelet into sensor's nodelet manager-->
|
|
||||||
<node pkg="nodelet" type="nodelet" name="pointcloud_to_laserscan" args="load pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet $(arg camera)_nodelet_manager">
|
|
||||||
|
|
||||||
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
|
|
||||||
<remap from="scan" to="$(arg camera)/scan"/>
|
|
||||||
<rosparam>
|
|
||||||
#target_frame: # Leave disabled to output scan in pointcloud frame
|
|
||||||
transform_tolerance: 0.01
|
|
||||||
min_height: 0.0
|
|
||||||
max_height: 1.0
|
|
||||||
|
|
||||||
angle_min: -1.5708 # -M_PI/2
|
|
||||||
angle_max: 1.5708 # M_PI/2
|
|
||||||
angle_increment: 0.087 # M_PI/360.0
|
|
||||||
scan_time: 0.3333
|
|
||||||
range_min: 0.45
|
|
||||||
range_max: 4.0
|
|
||||||
use_inf: true
|
|
||||||
|
|
||||||
# Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager
|
|
||||||
# 0 : Detect number of cores
|
|
||||||
# 1 : Single threaded
|
|
||||||
# 2->inf : Parallelism level
|
|
||||||
concurrency_level: 0
|
|
||||||
</rosparam>
|
|
||||||
|
|
||||||
</node>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
@ -1,11 +0,0 @@
|
|||||||
<library path="lib/libpointcloud_to_laserscan">
|
|
||||||
|
|
||||||
<class name="pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet"
|
|
||||||
type="pointcloud_to_laserscan::PointCloudToLaserScanNodelet"
|
|
||||||
base_class_type="nodelet::Nodelet">
|
|
||||||
<description>
|
|
||||||
Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans.
|
|
||||||
</description>
|
|
||||||
</class>
|
|
||||||
|
|
||||||
</library>
|
|
||||||
@ -1,37 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<package>
|
|
||||||
<name>pointcloud_to_laserscan</name>
|
|
||||||
<version>1.2.6</version>
|
|
||||||
<description>Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM).</description>
|
|
||||||
|
|
||||||
<maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
|
|
||||||
<author>Tully Foote</author>
|
|
||||||
|
|
||||||
<license>BSD</license>
|
|
||||||
|
|
||||||
<url type="website">http://ros.org/wiki/perception_pcl</url>
|
|
||||||
<url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
|
|
||||||
<url type="repository">https://github.com/ros-perception/perception_pcl</url>
|
|
||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
|
|
||||||
<build_depend>message_filters</build_depend>
|
|
||||||
<build_depend>nodelet</build_depend>
|
|
||||||
<build_depend>roscpp</build_depend>
|
|
||||||
<build_depend>sensor_msgs</build_depend>
|
|
||||||
<build_depend>tf2</build_depend>
|
|
||||||
<build_depend>tf2_ros</build_depend>
|
|
||||||
<build_depend>tf2_sensor_msgs</build_depend>
|
|
||||||
|
|
||||||
<run_depend>message_filters</run_depend>
|
|
||||||
<run_depend>nodelet</run_depend>
|
|
||||||
<run_depend>roscpp</run_depend>
|
|
||||||
<run_depend>sensor_msgs</run_depend>
|
|
||||||
<run_depend>tf2</run_depend>
|
|
||||||
<run_depend>tf2_ros</run_depend>
|
|
||||||
<run_depend>tf2_sensor_msgs</run_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<nodelet plugin="${prefix}/nodelets.xml"/>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,65 +0,0 @@
|
|||||||
/*
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2010-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 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: Paul Bovbel
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <nodelet/loader.h>
|
|
||||||
|
|
||||||
int main(int argc, char **argv){
|
|
||||||
ros::init(argc, argv, "pointcloud_to_laserscan_node");
|
|
||||||
ros::NodeHandle private_nh("~");
|
|
||||||
int concurrency_level;
|
|
||||||
private_nh.param<int>("concurrency_level", 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/pointcloud_to_laserscan_nodelet", remap, nargv);
|
|
||||||
|
|
||||||
boost::shared_ptr<ros::MultiThreadedSpinner> spinner;
|
|
||||||
if(concurrency_level) {
|
|
||||||
spinner.reset(new ros::MultiThreadedSpinner(concurrency_level));
|
|
||||||
}else{
|
|
||||||
spinner.reset(new ros::MultiThreadedSpinner());
|
|
||||||
}
|
|
||||||
spinner->spin();
|
|
||||||
return 0;
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,240 +0,0 @@
|
|||||||
/*
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2010-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 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: Paul Bovbel
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h>
|
|
||||||
#include <sensor_msgs/LaserScan.h>
|
|
||||||
#include <pluginlib/class_list_macros.h>
|
|
||||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
|
||||||
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
|
|
||||||
|
|
||||||
namespace pointcloud_to_laserscan
|
|
||||||
{
|
|
||||||
|
|
||||||
PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() {}
|
|
||||||
|
|
||||||
void PointCloudToLaserScanNodelet::onInit()
|
|
||||||
{
|
|
||||||
boost::mutex::scoped_lock lock(connect_mutex_);
|
|
||||||
private_nh_ = getPrivateNodeHandle();
|
|
||||||
|
|
||||||
private_nh_.param<std::string>("target_frame", target_frame_, "");
|
|
||||||
private_nh_.param<double>("tolerance", tolerance_, 0.01);
|
|
||||||
private_nh_.param<double>("min_height", min_height_, 0.0);
|
|
||||||
private_nh_.param<double>("max_height", max_height_, 1.0);
|
|
||||||
|
|
||||||
private_nh_.param<double>("angle_min", angle_min_, -M_PI / 2.0);
|
|
||||||
private_nh_.param<double>("angle_max", angle_max_, M_PI / 2.0);
|
|
||||||
private_nh_.param<double>("angle_increment", angle_increment_, M_PI / 360.0);
|
|
||||||
private_nh_.param<double>("scan_time", scan_time_, 1.0 / 30.0);
|
|
||||||
private_nh_.param<double>("range_min", range_min_, 0.45);
|
|
||||||
private_nh_.param<double>("range_max", range_max_, 4.0);
|
|
||||||
|
|
||||||
int concurrency_level;
|
|
||||||
private_nh_.param<int>("concurrency_level", concurrency_level, 1);
|
|
||||||
private_nh_.param<bool>("use_inf", use_inf_, true);
|
|
||||||
|
|
||||||
//Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size
|
|
||||||
if (concurrency_level == 1)
|
|
||||||
{
|
|
||||||
nh_ = getNodeHandle();
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
nh_ = getMTNodeHandle();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Only queue one pointcloud per running thread
|
|
||||||
if (concurrency_level > 0)
|
|
||||||
{
|
|
||||||
input_queue_size_ = concurrency_level;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
input_queue_size_ = boost::thread::hardware_concurrency();
|
|
||||||
}
|
|
||||||
|
|
||||||
// if pointcloud target frame specified, we need to filter by transform availability
|
|
||||||
if (!target_frame_.empty())
|
|
||||||
{
|
|
||||||
message_filter_.reset(new MessageFilter(sub_, tf2_, target_frame_, input_queue_size_, nh_));
|
|
||||||
message_filter_->setTolerance(ros::Duration(tolerance_));
|
|
||||||
message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
|
|
||||||
message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2));
|
|
||||||
}
|
|
||||||
else // otherwise setup direct subscription
|
|
||||||
{
|
|
||||||
sub_.registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
|
|
||||||
}
|
|
||||||
|
|
||||||
pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10,
|
|
||||||
boost::bind(&PointCloudToLaserScanNodelet::connectCb, this),
|
|
||||||
boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));
|
|
||||||
}
|
|
||||||
|
|
||||||
void PointCloudToLaserScanNodelet::connectCb()
|
|
||||||
{
|
|
||||||
boost::mutex::scoped_lock lock(connect_mutex_);
|
|
||||||
if (pub_.getNumSubscribers() > 0 && sub_.getSubscriber().getNumPublishers() == 0)
|
|
||||||
{
|
|
||||||
NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud");
|
|
||||||
sub_.subscribe(nh_, "cloud_in", input_queue_size_);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void PointCloudToLaserScanNodelet::disconnectCb()
|
|
||||||
{
|
|
||||||
boost::mutex::scoped_lock lock(connect_mutex_);
|
|
||||||
if (pub_.getNumSubscribers() == 0)
|
|
||||||
{
|
|
||||||
NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud");
|
|
||||||
sub_.unsubscribe();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
|
|
||||||
tf2_ros::filter_failure_reasons::FilterFailureReason reason)
|
|
||||||
{
|
|
||||||
NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to "
|
|
||||||
<< message_filter_->getTargetFramesString() << " with tolerance " << tolerance_);
|
|
||||||
}
|
|
||||||
|
|
||||||
void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
|
|
||||||
{
|
|
||||||
|
|
||||||
//build laserscan output
|
|
||||||
sensor_msgs::LaserScan output;
|
|
||||||
output.header = cloud_msg->header;
|
|
||||||
if (!target_frame_.empty())
|
|
||||||
{
|
|
||||||
output.header.frame_id = target_frame_;
|
|
||||||
}
|
|
||||||
|
|
||||||
output.angle_min = angle_min_;
|
|
||||||
output.angle_max = angle_max_;
|
|
||||||
output.angle_increment = angle_increment_;
|
|
||||||
output.time_increment = 0.0;
|
|
||||||
output.scan_time = scan_time_;
|
|
||||||
output.range_min = range_min_;
|
|
||||||
output.range_max = range_max_;
|
|
||||||
|
|
||||||
//determine amount of rays to create
|
|
||||||
uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
|
|
||||||
|
|
||||||
//determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
|
|
||||||
if (use_inf_)
|
|
||||||
{
|
|
||||||
output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
output.ranges.assign(ranges_size, output.range_max + 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
sensor_msgs::PointCloud2ConstPtr cloud_out;
|
|
||||||
sensor_msgs::PointCloud2Ptr cloud;
|
|
||||||
|
|
||||||
// Transform cloud if necessary
|
|
||||||
if (!(output.header.frame_id == cloud_msg->header.frame_id))
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
cloud.reset(new sensor_msgs::PointCloud2);
|
|
||||||
tf2_.transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_));
|
|
||||||
cloud_out = cloud;
|
|
||||||
}
|
|
||||||
catch (tf2::TransformException ex)
|
|
||||||
{
|
|
||||||
NODELET_ERROR_STREAM("Transform failure: " << ex.what());
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
cloud_out = cloud_msg;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Iterate through pointcloud
|
|
||||||
for (sensor_msgs::PointCloud2ConstIterator<float>
|
|
||||||
iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"), iter_z(*cloud_out, "z");
|
|
||||||
iter_x != iter_x.end();
|
|
||||||
++iter_x, ++iter_y, ++iter_z)
|
|
||||||
{
|
|
||||||
|
|
||||||
if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z))
|
|
||||||
{
|
|
||||||
NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (*iter_z > max_height_ || *iter_z < min_height_)
|
|
||||||
{
|
|
||||||
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
double range = hypot(*iter_x, *iter_y);
|
|
||||||
if (range < range_min_)
|
|
||||||
{
|
|
||||||
NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y,
|
|
||||||
*iter_z);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
double angle = atan2(*iter_y, *iter_x);
|
|
||||||
if (angle < output.angle_min || angle > output.angle_max)
|
|
||||||
{
|
|
||||||
NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
//overwrite range at laserscan ray if new range is smaller
|
|
||||||
int index = (angle - output.angle_min) / output.angle_increment;
|
|
||||||
if (range < output.ranges[index])
|
|
||||||
{
|
|
||||||
output.ranges[index] = range;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
pub_.publish(output);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet);
|
|
||||||
Loading…
x
Reference in New Issue
Block a user