Finalize pointcloud to laserscan
This commit is contained in:
Paul Bovbel
Paul Bovbel
parent
85241bcb53
commit
dc1cf04b07
@ -1,9 +1,6 @@
|
||||
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
|
||||
@ -14,21 +11,10 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
)
|
||||
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
|
||||
LIBRARIES PointCloudToLaserScan PointCloudToLaserScanNodelet
|
||||
CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs
|
||||
)
|
||||
|
||||
include_directories(
|
||||
@ -37,58 +23,22 @@ include_directories(
|
||||
)
|
||||
|
||||
add_library(PointCloudToLaserScan src/PointCloudToLaserScanBase.cpp)
|
||||
#add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg)
|
||||
target_link_libraries(PointCloudToLaserScan ${catkin_LIBRARIES})
|
||||
|
||||
add_library(PointCloudToLaserScanNodelet src/PointCloudToLaserScanNodelet.cpp)
|
||||
target_link_libraries(PointCloudToLaserScanNodelet 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(TARGETS PointCloudToLaserScan PointCloudToLaserScanNodelet pointcloud_to_laserscan
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
|
||||
#############
|
||||
## 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)
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
)
|
||||
install(FILES nodelets.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
@ -1,34 +1,41 @@
|
||||
/*
|
||||
* Copyright (c) 2012, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
* 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.
|
||||
*
|
||||
* * 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
|
||||
/*
|
||||
* Author: Paul Bovbel
|
||||
*/
|
||||
|
||||
#ifndef POINTCLOUD_TO_LASERSCAN_ROS
|
||||
@ -47,15 +54,18 @@
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
typedef pcl::PointXYZ Point;
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
|
||||
typedef pcl::PointCloud<Point> PointCloud;
|
||||
|
||||
/**
|
||||
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
|
||||
* pointcloud_to_laserscan implementation.
|
||||
*/
|
||||
class PointCloudToLaserScanBase
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
PointCloudToLaserScanBase(ros::NodeHandle& n, ros::NodeHandle& pnh, const unsigned int concurrency);
|
||||
PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh);
|
||||
|
||||
~PointCloudToLaserScanBase();
|
||||
|
||||
@ -76,13 +86,13 @@ private:
|
||||
tf::TransformListener tf_;
|
||||
|
||||
boost::mutex connect_mutex_;
|
||||
const unsigned int concurrency_;
|
||||
|
||||
unsigned int input_queue_size_;
|
||||
std::string target_frame_;
|
||||
float 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
|
||||
|
||||
24
pointcloud_to_laserscan/launch/sample_node.launch
Normal file
24
pointcloud_to_laserscan/launch/sample_node.launch
Normal file
@ -0,0 +1,24 @@
|
||||
<?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" name="pointcloud_to_laserscan">
|
||||
|
||||
<param name="use_inf" value="true"/>
|
||||
<param name="use_concurrency" value="true"/>
|
||||
|
||||
<param name="target_frame" value="base_link"/>
|
||||
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
|
||||
<remap from="scan" to="$(arg camera)/scan"/>
|
||||
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
24
pointcloud_to_laserscan/launch/sample_nodelet.launch
Normal file
24
pointcloud_to_laserscan/launch/sample_nodelet.launch
Normal file
@ -0,0 +1,24 @@
|
||||
<?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/PointCloudToLaserScanNodelet $(arg camera)_nodelet_manager">
|
||||
|
||||
<param name="use_inf" value="true"/>
|
||||
<param name="use_concurrency" value="true"/>
|
||||
|
||||
<param name="target_frame" value="base_link"/>
|
||||
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
|
||||
<remap from="scan" to="$(arg camera)/scan"/>
|
||||
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
11
pointcloud_to_laserscan/nodelets.xml
Normal file
11
pointcloud_to_laserscan/nodelets.xml
Normal file
@ -0,0 +1,11 @@
|
||||
<library path="lib/libPointCloudToLaserScanNodelet">
|
||||
|
||||
<class name="pointcloud_to_laserscan/PointCloudToLaserScanNodelet"
|
||||
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,44 +1,185 @@
|
||||
/*
|
||||
* 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/PointCloudToLaserScanBase.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
#include <pcl_ros/transforms.h>
|
||||
#include <math.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(ros::NodeHandle& nh, ros::NodeHandle& private_nh) :
|
||||
nh_(nh), private_nh_(private_nh)
|
||||
{
|
||||
|
||||
}
|
||||
private_nh.param<std::string>("target_frame", target_frame_, "base_link");
|
||||
private_nh_.param<double>("min_height", min_height_, 0.0);
|
||||
private_nh_.param<double>("max_height", max_height_, 1.0);
|
||||
|
||||
PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){
|
||||
sub_.shutdown();
|
||||
}
|
||||
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);
|
||||
|
||||
void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud){
|
||||
bool use_concurrency;
|
||||
private_nh_.param<bool>("use_concurrency", use_concurrency, true);
|
||||
private_nh_.param<bool>("use_inf", use_inf_, true);
|
||||
|
||||
}
|
||||
boost::mutex::scoped_lock lock(connect_mutex_);
|
||||
|
||||
//Only allow #threads pointclouds to wait in queue
|
||||
if(use_concurrency){
|
||||
input_queue_size_ = boost::thread::hardware_concurrency();
|
||||
}else{
|
||||
input_queue_size_ = 1;
|
||||
}
|
||||
|
||||
pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10,
|
||||
boost::bind(&PointCloudToLaserScanBase::connectCb, this),
|
||||
boost::bind(&PointCloudToLaserScanBase::disconnectCb, this));
|
||||
|
||||
|
||||
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.");
|
||||
PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){
|
||||
sub_.shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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", input_queue_size_, &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();
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud_msg){
|
||||
|
||||
//pointer to pointcloud data to transform to laserscan
|
||||
PointCloud::ConstPtr cloud_scan;
|
||||
|
||||
std_msgs::Header cloud_in_header = pcl_conversions::fromPCL(cloud_scan->header);
|
||||
|
||||
//decide if pointcloud needs to be transformed to a target frame
|
||||
if(!target_frame_.empty() && cloud_in_header.frame_id != target_frame_){
|
||||
bool found_transform = tf_.waitForTransform(cloud_in_header.frame_id, target_frame_, cloud_in_header.stamp, ros::Duration(10.0));
|
||||
if(found_transform){
|
||||
PointCloud::Ptr cloud_tf(new PointCloud);
|
||||
pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_);
|
||||
cloud_scan = cloud_tf;
|
||||
}else{
|
||||
ROS_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_in_header.frame_id << " into lasercan with frame " << target_frame_);
|
||||
return;
|
||||
}
|
||||
}else{
|
||||
cloud_scan = cloud_msg;
|
||||
}
|
||||
|
||||
//build laserscan output
|
||||
sensor_msgs::LaserScan output;
|
||||
output.header = cloud_in_header;
|
||||
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_;
|
||||
|
||||
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
|
||||
if(use_inf_){
|
||||
output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
|
||||
}else{
|
||||
output.ranges.assign(ranges_size, output.range_max + 1.0);
|
||||
}
|
||||
|
||||
for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){
|
||||
const float &x = it->x;
|
||||
const float &y = it->y;
|
||||
const float &z = it->z;
|
||||
|
||||
if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){
|
||||
//NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (z > max_height_ || z < min_height_){
|
||||
//NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_);
|
||||
continue;
|
||||
}
|
||||
|
||||
double range = hypot(x,y);
|
||||
if (range < range_min_){
|
||||
//NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z);
|
||||
continue;
|
||||
}
|
||||
|
||||
double angle = atan2(y, 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);
|
||||
|
||||
}
|
||||
|
||||
} // pointcloud_to_laserscan
|
||||
|
||||
@ -0,0 +1,76 @@
|
||||
/*
|
||||
* 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/PointCloudToLaserScanBase.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
|
||||
class PointCloudToLaserScanNodelet : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
PointCloudToLaserScanNodelet() {};
|
||||
|
||||
~PointCloudToLaserScanNodelet() {}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
bool use_concurrency;
|
||||
getPrivateNodeHandle().param<bool>("use_concurrency", use_concurrency, false);
|
||||
|
||||
if(use_concurrency){
|
||||
cloud_to_scan.reset(new PointCloudToLaserScanBase(getMTNodeHandle(), getPrivateNodeHandle()));
|
||||
}else{
|
||||
cloud_to_scan.reset(new PointCloudToLaserScanBase(getNodeHandle(), getPrivateNodeHandle()));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
boost::shared_ptr<PointCloudToLaserScanBase> cloud_to_scan;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet);
|
||||
|
||||
@ -1,16 +1,62 @@
|
||||
/*
|
||||
* 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 <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("~");
|
||||
ros::NodeHandle private_nh("~");
|
||||
bool use_concurrency;
|
||||
|
||||
unsigned int concurrency = 4;
|
||||
private_nh.param<bool>("use_concurrency", use_concurrency, false);
|
||||
|
||||
pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, nh_private, concurrency);
|
||||
pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, private_nh);
|
||||
|
||||
ros::spin();
|
||||
if(use_concurrency) {
|
||||
ros::MultiThreadedSpinner spinner;
|
||||
spinner.spin();
|
||||
}else{
|
||||
ros::spin();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user