cob_unified_scan_publisher.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_navigation
00012  * ROS package name: cob_unified_scan_publisher
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Florian Mirus, email:florian.mirus@ipa.fhg.de
00018  * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
00019  *
00020  * Date of creation: Jan 2011
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 //##################
00055 //#### includes ####
00056 
00057 // standard includes
00058 //--
00059 
00060 // ROS includes
00061 #include <ros/ros.h>
00062 #include <message_filters/subscriber.h>
00063 #include <message_filters/time_synchronizer.h>
00064 #include <tf/transform_listener.h>
00065 #include <sensor_msgs/PointCloud.h>
00066 #include <laser_geometry/laser_geometry.h>
00067 
00068 // ROS message includes
00069 #include <sensor_msgs/LaserScan.h>
00070 
00071 
00072 //####################
00073 //#### node class ####
00074 class NodeClass
00075 {
00076     //
00077     public:
00078           
00079                 ros::NodeHandle nodeHandle;   
00080         // topics to publish
00081         message_filters::Subscriber<sensor_msgs::LaserScan> * topicSub_LaserFront;
00082                 message_filters::Subscriber<sensor_msgs::LaserScan> * topicSub_LaserBack;
00083         message_filters::TimeSynchronizer<sensor_msgs::LaserScan, sensor_msgs::LaserScan> *sync;
00084                 tf::TransformListener listener_;
00085                 laser_geometry::LaserProjection projector_;
00086                 ros::Publisher topicPub_LaserUnified;
00087 
00088                 tf::StampedTransform transform_scan_front;
00089             tf::StampedTransform transform_scan_back;
00090           
00091 
00092     NodeClass()
00093     {
00094       // loading config
00095       // implementation of topics to publish
00096       topicPub_LaserUnified = nodeHandle.advertise<sensor_msgs::LaserScan>("scan_unified", 1);
00097       topicSub_LaserFront = new message_filters::Subscriber<sensor_msgs::LaserScan>(nodeHandle, "/scan_front", 1);
00098 
00099           topicSub_LaserBack = new message_filters::Subscriber<sensor_msgs::LaserScan>(nodeHandle, "/scan_rear", 1);
00100           sync = new message_filters::TimeSynchronizer<sensor_msgs::LaserScan, sensor_msgs::LaserScan>(*topicSub_LaserFront, *topicSub_LaserBack, 10);
00101           sync->registerCallback(boost::bind(&NodeClass::scanCallback, this, _1, _2));
00102 
00103           //listener_.lookupTransform(scan_front->header.frame_id, "base_link",  ros::Time(0), transform_scan_front);
00104           //listener_.lookupTransform(scan_rear->header.frame_id, "base_link",  ros::Time(0), transform_scan_back);
00105 
00106     }
00107 
00108     void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_front, const sensor_msgs::LaserScan::ConstPtr& scan_rear)
00109     {
00110       // create LaserScan message
00111           sensor_msgs::LaserScan laserUnified;
00112           sensor_msgs::PointCloud cloud_front;
00113           //ROS_INFO("Converting front scan to point cloud");
00114           listener_.waitForTransform("/base_link", scan_front->header.frame_id, scan_front->header.stamp, ros::Duration(15.0));
00115           projector_.transformLaserScanToPointCloud("/base_link",*scan_front, cloud_front, listener_);
00116           sensor_msgs::PointCloud cloud_rear;
00117           //ROS_INFO("Converting rear scan to point cloud");
00118           listener_.waitForTransform("/base_link", scan_rear->header.frame_id, scan_rear->header.stamp, ros::Duration(15.0));
00119           projector_.transformLaserScanToPointCloud("/base_link",*scan_rear, cloud_rear, listener_);
00120 
00121           //ROS_INFO("Creating message header");
00122           laserUnified.header = scan_front->header;
00123       laserUnified.header.frame_id = "base_link";
00124           laserUnified.angle_min = -M_PI+0.1;
00125           laserUnified.angle_max = M_PI-0.1;
00126           laserUnified.angle_increment = M_PI/180.0/2.0;
00127           laserUnified.time_increment = 0.0;
00128           laserUnified.scan_time = scan_front->scan_time;
00129           laserUnified.range_min = scan_front->range_min;
00130           laserUnified.range_max = scan_front->range_max;
00131           laserUnified.ranges.resize((laserUnified.angle_max - laserUnified.angle_min) / laserUnified.angle_increment);
00132           //ROS_INFO("Converting scan from point cloud");
00133           for (unsigned int i = 0; i < cloud_front.points.size(); i++)
00134       {
00135         const float &x = cloud_front.points[i].x;
00136         const float &y = cloud_front.points[i].y;
00137         const float &z = cloud_front.points[i].z;
00138                 //ROS_INFO("Point %f %f %f", x, y, z);
00139                 if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
00140         {
00141                         ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
00142                 continue;
00143                 }
00144                 double angle = atan2(y, x);// + M_PI/2;
00145         if (angle < laserUnified.angle_min || angle > laserUnified.angle_max)
00146         {
00147                 ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, laserUnified.angle_min, laserUnified.angle_max);
00148                 continue;
00149         }
00150                 int index = (angle - laserUnified.angle_min) / laserUnified.angle_increment;
00151                 double range_sq = y*y+x*x;
00152         //printf ("index xyz( %f %f %f) angle %f index %d range %f\n", x, y, z, angle, index, sqrt(range_sq));
00153         laserUnified.ranges[index] = sqrt(range_sq);
00154           }
00155           for (unsigned int i = 0; i < cloud_rear.points.size(); i++)
00156       {
00157         const float &x = cloud_rear.points[i].x;
00158         const float &y = cloud_rear.points[i].y;
00159         const float &z = cloud_rear.points[i].z;
00160                 //ROS_INFO("Point %f %f %f", x, y, z);
00161                 if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
00162         {
00163                         ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
00164                 continue;
00165                 }
00166                 double angle = atan2(y, x); //+ M_PI/2; //TODO: no ideas why but it works
00167                 
00168         if (angle < laserUnified.angle_min || angle > laserUnified.angle_max)
00169         {
00170                 ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, laserUnified.angle_min, laserUnified.angle_max);
00171                 continue;
00172         }
00173                 int index = (angle - laserUnified.angle_min) / laserUnified.angle_increment;
00174                 double range_sq = y*y+x*x;
00175         //printf ("index xyz( %f %f %f) angle %f index %d range %f\n", x, y, z, angle, index, sqrt(range_sq));
00176         laserUnified.ranges[index] = sqrt(range_sq);
00177           }
00178 
00179           topicPub_LaserUnified.publish(laserUnified);
00180     }
00181 };
00182 
00183 //#######################
00184 //#### main programm ####
00185 int main(int argc, char** argv)
00186 {
00187   // initialize ROS, spezify name of node
00188   ros::init(argc, argv, "scanner_unifier");
00189 
00190   NodeClass nc;
00191 
00192   ros::spin();
00193   return 0;
00194 }
00195 


cob_unified_scan_publisher
Author(s): Florian Mirus
autogenerated on Fri Aug 28 2015 10:22:20