env_model_loader.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ###############################################################################
00003 # \file
00004 #
00005 # $Id:$
00006 #
00007 # Copyright (C) Brno University of Technology
00008 #
00009 # This file is part of software developed by dcgm-robotics@FIT group.
00010 # 
00011 # Author: Zdenek Materna (imaterna@fit.vutbr.cz)
00012 # Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00013 # Date: dd/mm/2012
00014 #
00015 # This file is free software: you can redistribute it and/or modify
00016 # it under the terms of the GNU Lesser General Public License as published by
00017 # the Free Software Foundation, either version 3 of the License, or
00018 # (at your option) any later version.
00019 # 
00020 # This file is distributed in the hope that it will be useful,
00021 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00022 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023 # GNU Lesser General Public License for more details.
00024 # 
00025 # You should have received a copy of the GNU Lesser General Public License
00026 # along with this file.  If not, see <http://www.gnu.org/licenses/>.
00027 #
00028 
00029 import roslib; roslib.load_manifest('srs_user_tests')
00030 import rospy
00031 from srs_env_model.srv import LoadSave, ResetOctomap
00032 import sys
00033 import os
00034 from std_msgs.msg import Empty as EmptyMsg
00035 
00036 class LoaderSaver():
00037     
00038     def __init__(self):
00039         
00040         self.loader = rospy.ServiceProxy("/but_env_model/load_octomap_full", LoadSave)
00041         self.saver = rospy.ServiceProxy("/but_env_model/save_octomap_full", LoadSave)
00042         
00043     def load(self,fname):
00044         
00045         rospy.loginfo("Trying to load octomap (from %s)",str(fname))
00046         
00047         available = False
00048     
00049         try:
00050             
00051             rospy.wait_for_service("/but_env_model/load_octomap_full",timeout=60)
00052             available = True
00053             
00054         except ROSException, e:
00055             
00056            pass
00057        
00058         if not available:
00059            
00060            rospy.logerr("Service is not available")
00061            return
00062         
00063         try:
00064             
00065             res = self.loader(filename = fname)
00066             
00067         except Exception, e:
00068         
00069             rospy.logerr('Cannot load the octomap, error: %s',str(e))
00070             
00071         if res.all_ok:
00072             
00073             rospy.loginfo("The octomap has been loaded.")
00074         
00075         
00076     def save(self,fname):
00077         
00078         rospy.loginfo("Trying to save octomap (to %s)",str(fname))
00079         
00080         available = False
00081     
00082         try:
00083             
00084             rospy.wait_for_service("/but_env_model/save_octomap_full",timeout=60)
00085             available = True
00086             
00087         except ROSException, e:
00088             
00089            pass
00090        
00091         if not available:
00092            
00093            rospy.logerr("Service is not available")
00094            return
00095         
00096         try:
00097     
00098             res = self.saver(filename = fname)
00099             
00100         except Exception, e:
00101         
00102             rospy.logerr('Cannot save the octomap, error: %s',str(e))
00103             
00104         if res.all_ok:
00105             
00106             rospy.loginfo("The octomap has been saved.")
00107         
00108         
00109         
00110 
00111 if __name__ == '__main__':
00112 
00113   try:
00114       
00115     rospy.init_node('octomap_loader')
00116       
00117     l = LoaderSaver()
00118     
00119     
00120     if not rospy.has_param('~action'):
00121         
00122         rospy.logerr('Param "action" is not set')
00123         sys.exit()
00124         
00125     if not rospy.has_param('~file'):
00126         
00127         rospy.logerr('Param "file" is not set')
00128         sys.exit()
00129 
00130 
00131     action = str(rospy.get_param('~action'))
00132     file = str(rospy.get_param('~file'))
00133     
00134     if action == "load":
00135         
00136         if not os.path.exists(file):
00137         
00138             rospy.logerr("File (%s) does not exist!",file)
00139             sys.exit()
00140             
00141         sim = rospy.get_param('/use_sim_time')
00142     
00143         if sim is True:
00144         
00145           rospy.loginfo('Waiting until simulation is ready...')
00146     
00147           rospy.wait_for_message('/sim_init',EmptyMsg)
00148           
00149           rospy.wait_for_service("/but_env_model/reset_octomap")
00150           
00151           reset = rospy.ServiceProxy("/but_env_model/reset_octomap", ResetOctomap)
00152           
00153           try:
00154     
00155             res = reset()
00156             
00157             rospy.loginfo('Reset of octomap takes some time, be patient.')
00158             
00159           except Exception, e:
00160         
00161             rospy.logerr('Cannot reset octomap, error: %s',str(e))
00162           
00163         rospy.sleep(10)
00164         
00165         l.load(file)
00166         
00167     elif action == "save":
00168     
00169         l.save(file)
00170         
00171     else:
00172         
00173         rospy.logerr("Unknown action. Please use 'load' or 'save'.")
00174     
00175   except rospy.ROSInterruptException:
00176     pass


srs_user_tests
Author(s): SRS team
autogenerated on Mon Oct 6 2014 08:53:11