periodic_snapshotter.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 <cstdio>
00036 #include <ros/ros.h>
00037 
00038 // Services
00039 #include "laser_assembler/AssembleScans.h"
00040 
00041 // Messages
00042 #include "sensor_msgs/PointCloud.h"
00043 
00044 
00045 /***
00046  * This a simple test app that requests a point cloud from the
00047  * point_cloud_assembler every 4 seconds, and then publishes the
00048  * resulting data
00049  */
00050 namespace laser_assembler
00051 {
00052 
00053 class PeriodicSnapshotter
00054 {
00055 
00056 public:
00057 
00058   PeriodicSnapshotter()
00059   {
00060     // Create a publisher for the clouds that we assemble
00061     pub_ = n_.advertise<sensor_msgs::PointCloud> ("assembled_cloud", 1);
00062 
00063     // Create the service client for calling the assembler
00064     client_ = n_.serviceClient<AssembleScans>("assemble_scans");
00065 
00066     // Start the timer that will trigger the processing loop (timerCallback)
00067     timer_ = n_.createTimer(ros::Duration(5,0), &PeriodicSnapshotter::timerCallback, this);
00068 
00069     // Need to track if we've called the timerCallback at least once
00070     first_time_ = true;
00071   }
00072 
00073   void timerCallback(const ros::TimerEvent& e)
00074   {
00075 
00076     // We don't want to build a cloud the first callback, since we we
00077     //   don't have a start and end time yet
00078     if (first_time_)
00079     {
00080       first_time_ = false;
00081       return;
00082     }
00083 
00084     // Populate our service request based on our timer callback times
00085     AssembleScans srv;
00086     srv.request.begin = e.last_real;
00087     srv.request.end   = e.current_real;
00088 
00089     // Make the service call
00090     if (client_.call(srv))
00091     {
00092       ROS_INFO("Published Cloud with %u points", (uint32_t)(srv.response.cloud.points.size())) ;
00093       pub_.publish(srv.response.cloud);
00094     }
00095     else
00096     {
00097       ROS_ERROR("Error making service call\n") ;
00098     }
00099   }
00100 
00101 private:
00102   ros::NodeHandle n_;
00103   ros::Publisher pub_;
00104   ros::ServiceClient client_;
00105   ros::Timer timer_;
00106   bool first_time_;
00107 } ;
00108 
00109 }
00110 
00111 using namespace laser_assembler ;
00112 
00113 int main(int argc, char **argv)
00114 {
00115   ros::init(argc, argv, "periodic_snapshotter");
00116   ros::NodeHandle n;
00117   ROS_INFO("Waiting for [build_cloud] to be advertised");
00118   ros::service::waitForService("build_cloud");
00119   ROS_INFO("Found build_cloud! Starting the snapshotter");
00120   PeriodicSnapshotter snapshotter;
00121   ros::spin();
00122   return 0;
00123 }


laser_assembler
Author(s): Vijay Pradeep
autogenerated on Thu Aug 27 2015 13:47:32