dense_laser_imager.cpp
Go to the documentation of this file.
00001 
00002 /*********************************************************************
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00040 #include <ros/ros.h>
00041 #include "sensor_msgs/Image.h"
00042 #include "calibration_msgs/DenseLaserSnapshot.h"
00043 
00044 using namespace std ;
00045 using namespace ros ;
00046 
00047 
00051 class LaserImager
00052 {
00053 public:
00054 
00055   LaserImager()
00056   {
00057     sub_ = n_.subscribe("dense_laser_snapshot", 1, &LaserImager::snapshotCallback, this) ;
00058     intensity_pub_ = n_.advertise<sensor_msgs::Image> ("dense_laser_intensity", 1) ;
00059     range_pub_ = n_.advertise<sensor_msgs::Image> ("dense_laser_intensity", 1) ;
00060 
00061   }
00062 
00063   void snapshotCallback(const calibration_msgs::DenseLaserSnapshotConstPtr& msg)
00064   {
00065     sensor_msgs::Image image ;
00066     const unsigned int N = msg->num_scans * msg->readings_per_scan ;
00067 
00068 
00069     image.header.stamp = msg->header.stamp ;
00070     image.encoding = "mono8" ;
00071     image.height = msg->num_scans;
00072     image.width = msg->readings_per_scan;
00073     image.step = msg->readings_per_scan;
00074 
00075     image.data.resize(msg->num_scans*msg->readings_per_scan) ;
00076 
00077     for(unsigned int i=0; i<N; i++)
00078     {
00079       image.data[i] = (unsigned int) ((fmin(3000, fmax(2000, msg->intensities[i])) - 2000) / 1000.0 * 255) ;
00080     }
00081     intensity_pub_.publish(image) ;
00082   }
00083 
00084 private:
00085   NodeHandle n_ ;
00086   Subscriber sub_ ;
00087   Publisher intensity_pub_ ;
00088   Publisher range_pub_ ;
00089 
00090 };
00091 
00092 int main(int argc, char** argv)
00093 {
00094   ros::init(argc, argv, "laser_imager") ;
00095 
00096   LaserImager imager ;
00097 
00098   ros::spin() ;
00099 
00100   return 0 ;
00101 }


dense_laser_assembler
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:29