laser_scan_sparsifier.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Ivan Dryanovski, William Morris
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the CCNY Robotics Lab nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "laser_scan_sparsifier/laser_scan_sparsifier.h"
00031 
00032 namespace scan_tools {
00033 
00034 LaserScanSparsifier::LaserScanSparsifier(ros::NodeHandle nh, ros::NodeHandle nh_private):
00035   nh_(nh), 
00036   nh_private_(nh_private)
00037 {
00038   ROS_INFO ("Starting LaserScanSparsifier");
00039 
00040   // **** get paramters
00041 
00042   if (!nh_private_.getParam ("step", step_))
00043     step_ = 2;
00044 
00045   ROS_ASSERT_MSG(step_ > 0,
00046     "step parameter is set to %, must be > 0", step_);
00047 
00048   // **** advertise topics
00049 
00050   scan_publisher_ = nh_.advertise<sensor_msgs::LaserScan>(
00051     "scan_sparse", 10);
00052 
00053   // **** subscribe to laser scan messages
00054 
00055   scan_subscriber_ = nh_.subscribe(
00056     "scan", 10, &LaserScanSparsifier::scanCallback, this);
00057 }
00058 
00059 LaserScanSparsifier::~LaserScanSparsifier ()
00060 {
00061   ROS_INFO ("Destroying LaserScanSparsifier");
00062 }
00063 
00064 void LaserScanSparsifier::scanCallback (const sensor_msgs::LaserScanConstPtr& scan_msg)
00065 {
00066   sensor_msgs::LaserScan::Ptr scan_sparse;
00067   scan_sparse = boost::make_shared<sensor_msgs::LaserScan>();
00068 
00069   // copy over equal fields
00070 
00071   scan_sparse->header          = scan_msg->header;
00072   scan_sparse->range_min       = scan_msg->range_min;
00073   scan_sparse->range_max       = scan_msg->range_max;
00074   scan_sparse->angle_min       = scan_msg->angle_min;
00075   scan_sparse->angle_increment = scan_msg->angle_increment * step_;
00076   scan_sparse->time_increment  = scan_msg->time_increment;
00077   scan_sparse->scan_time       = scan_msg->scan_time;
00078 
00079   // determine size of new scan
00080 
00081   unsigned int size_sparse = scan_msg->ranges.size() / step_;
00082   scan_sparse->ranges.resize(size_sparse);
00083 
00084   // determine new maximum angle
00085 
00086   scan_sparse->angle_max = 
00087     scan_sparse->angle_min + (scan_sparse->angle_increment * (size_sparse - 1));
00088 
00089   for (unsigned int i = 0; i < size_sparse; i++)
00090   {
00091     scan_sparse->ranges[i] = scan_msg->ranges[i * step_];
00092     // TODO - also copy intensity values
00093   }
00094 
00095   scan_publisher_.publish(scan_sparse);
00096 }
00097 
00098 } //namespace scan_tools
00099 


laser_scan_sparsifier
Author(s): Ivan Dryanovski, William Morris
autogenerated on Thu Mar 23 2017 03:34:33