laser_scan_splitter.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_splitter/laser_scan_splitter.h"
00031 
00032 namespace scan_tools {
00033 
00034 LaserScanSplitter::LaserScanSplitter(ros::NodeHandle nh, ros::NodeHandle nh_private):
00035   nh_(nh), 
00036   nh_private_(nh_private)
00037 {
00038   ROS_INFO ("Starting LaserScanSplitter");
00039 
00040   // **** get paramters
00041 
00042   std::string topics_string;
00043   std::string frames_string;
00044   std::string sizes_string;
00045 
00046   if (!nh_private_.getParam ("topics", topics_string))
00047     topics_string = "scan1 scan2";
00048   if (!nh_private_.getParam ("frames", frames_string))
00049     frames_string = "laser laser";
00050   if (!nh_private_.getParam ("sizes", sizes_string))
00051     sizes_string = "256 256";
00052 
00053   // **** tokenize inputs
00054   tokenize (topics_string, published_scan_topics_);
00055   tokenize (frames_string, published_laser_frames_);
00056 
00057   std::vector<std::string> sizes_tokens;
00058   tokenize (sizes_string, sizes_tokens);
00059 
00060   size_sum_ = 0;
00061   for (unsigned int i = 0; i < sizes_tokens.size (); i++)
00062   {
00063     sizes_.push_back (atoi (sizes_tokens[i].c_str ()));
00064     size_sum_ += sizes_[i];
00065 
00066     ROS_ASSERT_MSG ((sizes_[i] > 0), "LaserScanSplitter: Scan size cannot be zero. Quitting.");
00067   }
00068 
00069   // **** check that topic, frames, and sizes vectors have same sizes
00070 
00071   ROS_ASSERT_MSG ((published_scan_topics_.size () == published_laser_frames_.size ()) &&
00072                   (sizes_.size () == published_laser_frames_.size ()),
00073                   "LaserScanSplitter: Invalid parameters. Quitting.");
00074 
00075   // **** subscribe to laser scan messages
00076   scan_subscriber_ = nh_.subscribe (scan_topic_, 10, &LaserScanSplitter::scanCallback, this);
00077 
00078   // **** advertise topics
00079   for (unsigned int i = 0; i < published_scan_topics_.size (); i++)
00080   {
00081     scan_publishers_.push_back (ros::Publisher ());
00082     scan_publishers_[i] = 
00083       nh_.advertise<sensor_msgs::LaserScan>(published_scan_topics_[i], 10);
00084   }
00085 }
00086 
00087 LaserScanSplitter::~LaserScanSplitter ()
00088 {
00089   ROS_INFO ("Destroying LaserScanSplitter");
00090 }
00091 
00092 void LaserScanSplitter::scanCallback (const sensor_msgs::LaserScanConstPtr & scan_msg)
00093 {
00094   // **** check for scan size
00095   if (size_sum_ != scan_msg->ranges.size ())
00096   {
00097     ROS_WARN ("LaserScanSplitter: Received a laser scan with size (%d) \
00098       incompatible to input parameters. Skipping scan.", (int)scan_msg->ranges.size());
00099     return;
00100   }
00101 
00102   // **** copy information over
00103   int r = 0;
00104 
00105   for (unsigned int i = 0; i < published_scan_topics_.size (); i++)
00106   {
00107     sensor_msgs::LaserScan::Ptr scan_segment;
00108     scan_segment = boost::make_shared<sensor_msgs::LaserScan>();
00109 
00110     scan_segment->header = scan_msg->header;
00111     scan_segment->range_min = scan_msg->range_min;
00112     scan_segment->range_max = scan_msg->range_max;
00113     scan_segment->angle_increment = scan_msg->angle_increment;
00114     scan_segment->time_increment = scan_msg->time_increment;
00115     scan_segment->scan_time = scan_msg->scan_time;
00116     scan_segment->header.frame_id = published_laser_frames_[i];
00117 
00118     scan_segment->angle_min = 
00119       scan_msg->angle_min + (scan_msg->angle_increment * r);
00120     scan_segment->angle_max = 
00121       scan_msg->angle_min + (scan_msg->angle_increment * (r + sizes_[i] - 1));
00122 
00123     // TODO - also copy intensity values
00124 
00125     scan_segment->ranges.resize(sizes_[i]);
00126     memcpy(&scan_segment->ranges[0], &scan_msg->ranges[r], sizes_[i]*sizeof(int));
00127     r+=sizes_[i];
00128 
00129     scan_publishers_[i].publish (scan_segment);
00130   }
00131 }
00132 
00133 void LaserScanSplitter::tokenize (const std::string & str, std::vector < std::string > &tokens)
00134 {
00135   std::string::size_type last_pos = str.find_first_not_of (" ", 0);
00136   std::string::size_type pos = str.find_first_of (" ", last_pos);
00137 
00138   while (std::string::npos != pos || std::string::npos != last_pos)
00139   {
00140     std::string string_token = str.substr (last_pos, pos - last_pos);
00141     tokens.push_back (string_token);
00142     last_pos = str.find_first_not_of (" ", pos);
00143     pos = str.find_first_of (" ", last_pos);
00144   }
00145 }
00146 
00147 } //namespace scan_tools
00148 


laser_scan_splitter
Author(s): Ivan Dryanovski, William Morris
autogenerated on Thu Jun 6 2019 22:03:32