laser_interval_calc_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <ros/ros.h>
00036 #include <calibration_msgs/IntervalStamped.h>
00037 #include <laser_cb_detector/laser_interval_calc.h>
00038 #include <message_filters/subscriber.h>
00039 #include <message_filters/time_synchronizer.h>
00040 
00041 using namespace laser_cb_detector;
00042 
00043 void syncCallback(ros::Publisher* pub,
00044                   const calibration_msgs::DenseLaserSnapshotConstPtr& snapshot,
00045                   const calibration_msgs::CalibrationPatternConstPtr& features)
00046 {
00047   calibration_msgs::IntervalStamped out;
00048 
00049   bool success;
00050 
00051   success = LaserIntervalCalc::computeInterval(*snapshot, *features, out.interval);
00052 
00053   if (!success)
00054   {
00055     ROS_WARN("Failed trying to compute interval. Not going to publish");
00056     return;
00057   }
00058 
00059   out.header.stamp = snapshot->header.stamp;
00060   pub->publish(out);
00061 }
00062 
00063 int main(int argc, char** argv)
00064 {
00065   ros::init(argc, argv, "laser_interval_calc");
00066 
00067   ros::NodeHandle nh;
00068 
00069   ros::Publisher pub = nh.advertise<calibration_msgs::IntervalStamped>("laser_interval", 1);
00070 
00071   message_filters::Subscriber<calibration_msgs::DenseLaserSnapshot> snapshot_sub(nh, "snapshot", 1);
00072   message_filters::Subscriber<calibration_msgs::CalibrationPattern> features_sub(nh, "features", 1);
00073   message_filters::TimeSynchronizer<calibration_msgs::DenseLaserSnapshot,
00074                                     calibration_msgs::CalibrationPattern> sync(snapshot_sub, features_sub, 2);
00075   sync.registerCallback(boost::bind(&syncCallback, &pub, _1, _2));
00076 
00077   ros::spin();
00078 
00079   return 0;
00080 }


laser_cb_detector
Author(s): Vijay Pradeep
autogenerated on Tue Sep 27 2016 04:06:46