topicLogger_server.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Robert Bosch LLC
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 Willow Garage, Inc. 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 //\Author Ralf Kempf, Robert Bosch LLC revised Sarah Osentoski for electric
00036 
00037 #include <ros/ros.h>
00038 #include <ros/node_handle.h>
00039 #include <actionlib/server/simple_action_server.h>
00040 #include <topic_logger/TopicLoggerAction.h>
00041 #include <boost/thread.hpp>
00042 #include <boost/thread/mutex.hpp>
00043 #include <boost/bind.hpp>
00044 #include <boost/program_options.hpp>
00045 #include <vector>
00046 #include <topic_logger/recorder.h>
00047 #include <unistd.h>
00048 
00049 using namespace std;
00050 
00051 class TopicLoggerAction
00052 {
00053 protected:
00054 
00055         ros::NodeHandle nh_;
00056         actionlib::SimpleActionServer<topic_logger::TopicLoggerAction> as_;
00057 
00058         string action_name_;
00059         //create messages that are used to published feedback/result
00060         topic_logger::TopicLoggerFeedback feedback_;
00061         topic_logger::TopicLoggerResult result_;
00062 
00063         rosbag::Recorder recorder;
00064 
00065 public:
00066 
00067         std::string state;
00068 
00069         TopicLoggerAction(string name):
00070                 as_(nh_, name, boost::bind(&TopicLoggerAction::executeCB, this, _1), false),
00071                 action_name_(name)
00072                 {
00073                                 as_.start();
00074                                 ROS_INFO("Server is up");
00075                 }
00076 
00077         ~TopicLoggerAction(void)
00078         {
00079         }
00080 
00081         void executeStart(const topic_logger::TopicLoggerGoalConstPtr &goal)
00082         {
00083                 //set rate for publishing feedback
00084                 ros::Rate r(1);
00085                 //decide what to to considering the current state
00086                 if (state == "idle")
00087                 {
00088                         recorder.options_.topics = goal->selectedTopics;
00089                         recorder.run();
00090                         state = "recording";
00091                 }
00092                 else if (state == "recording")
00093                 {
00094                         recorder.stop();
00095                         recorder.deleteFile();
00096                         recorder.options_.topics = goal->selectedTopics;
00097                         recorder.run();
00098                         state = "recording";
00099                 }
00100                 else
00101                 {
00102                         return;
00103                 }
00104                 feedback_.filesize = 0.0;
00105                 while (ros::ok())
00106                 {
00107                         //check if preemption is requested
00108                         if (as_.isPreemptRequested())
00109                         {
00110                                 as_.setPreempted();
00111                                 break;
00112                         }
00113                         else
00114                         {
00115                                 //publish current filesize as feedback
00116                                 feedback_.filesize = recorder.bag_.getSize();
00117                                 as_.publishFeedback(feedback_);
00118                                 r.sleep();
00119                         }
00120                 }
00121         }
00122 
00123         void executeStop()
00124         {
00125                 state = "uploading";
00126 
00127                 ros::NodeHandle nh;
00128                 recorder.stop();
00129                 ROS_INFO("Uploading file ...");
00130                 recorder.upload();
00131                 recorder.deleteFile();
00132 
00133                 //send result (filename and link) to simple action client
00134                 result_.target_filename = recorder.target_filename_.c_str();
00135                 std::string htmllink;
00136                 result_.downloadURL = "";
00137                 if(!nh.getParam("BaseURLWebserver_topicLogger", htmllink))
00138                 {
00139                         ROS_INFO("Failed to load 'BaseURLWebserver_topicLogger'. No Link available");
00140                 }
00141                 else{
00142                         std::string tmp;
00143                         tmp = htmllink + recorder.target_filename_;
00144                         result_.downloadURL = tmp.c_str();
00145                 }
00146                 as_.setSucceeded(result_);
00147                 ROS_INFO("================================");
00148 
00149                 state = "idle";
00150         }
00151 
00152         void executeCB(const topic_logger::TopicLoggerGoalConstPtr &goal)
00153         {
00154                 //decide whether recording should be started or stopped
00155                 if (goal->command == "start")
00156                 {
00157                         executeStart(goal);
00158                 }
00159                 else if (goal->command == "stop")
00160                 {
00161                         executeStop();
00162                 }
00163         }
00164 };
00165 
00166 int main(int argc, char** argv)
00167 {
00168         ros::init(argc, argv, "topicLogger");
00169         ros::NodeHandle nh;
00170 
00171         //get local work directory from param server and change LWD
00172         std::string localWD;
00173         if(nh.getParam("localWorkDirectory_topicLogger", localWD))
00174         {
00175                 //change the work directory so that the local directory for saving the .bag file is correct
00176                 int i;
00177                 i = chdir(localWD.c_str());
00178                 if (i != 0)
00179                         ROS_INFO("Changing the work directory failed");
00180                 else ROS_INFO("Local work directory is: %s", localWD.c_str());
00181         }
00182         else ROS_INFO("Failed to load 'localWorkDirectory_topicLogger'.");
00183 
00184         TopicLoggerAction topicLogger(ros::this_node::getName());
00185         topicLogger.state = "idle";
00186         ros::MultiThreadedSpinner s(10);
00187         ros::spin();
00188 
00189         return 0;
00190 }


topic_logger
Author(s): Ralf Kempf Maintained by Sarah Osentoski and Ben Pitzer
autogenerated on Sun Jan 5 2014 11:33:05