geom_map_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
00032 import sys
00033 import os
00034 
00035 class LoaderSaver():
00036     
00037     def __init__(self):
00038         
00039         self.loader = rospy.ServiceProxy("/but_plane_detector/load_planes", LoadSave)
00040         self.saver = rospy.ServiceProxy("/but_plane_detector/save_planes", LoadSave)
00041         
00042     def load(self,fname):
00043         
00044         rospy.loginfo("Trying to load geometric map (from %s)",str(fname))
00045         
00046         available = False
00047     
00048         try:
00049             
00050             rospy.wait_for_service("/but_plane_detector/load_planes",timeout=60)
00051             available = True
00052             
00053         except ROSException, e:
00054             
00055            pass
00056        
00057         if not available:
00058            
00059            rospy.logerr("Service is not available")
00060            return
00061         
00062         try:
00063             
00064             res = self.loader(filename = fname)
00065             
00066         except Exception, e:
00067         
00068             rospy.logerr('Cannot load geometric map, error: %s',str(e))
00069             
00070         if res.all_ok:
00071             
00072             rospy.loginfo("Geometric map has been loaded.")
00073         
00074         
00075     def save(self,fname):
00076         
00077         rospy.loginfo("Trying to save geometric map (to %s)",str(fname))
00078         
00079         available = False
00080     
00081         try:
00082             
00083             rospy.wait_for_service("/but_plane_detector/save_planes",timeout=60)
00084             available = True
00085             
00086         except ROSException, e:
00087             
00088            pass
00089        
00090         if not available:
00091            
00092            rospy.logerr("Service is not available")
00093            return
00094         
00095         try:
00096     
00097             res = self.saver(filename = fname)
00098             
00099         except Exception, e:
00100         
00101             rospy.logerr('Cannot save the geometric map, error: %s',str(e))
00102             
00103         if res.all_ok:
00104             
00105             rospy.loginfo("The geometric map has been saved.")
00106         
00107         
00108         
00109 
00110 if __name__ == '__main__':
00111 
00112   try:
00113       
00114     rospy.init_node('geommap_loader')
00115       
00116     l = LoaderSaver()
00117     
00118     if not rospy.has_param('~action'):
00119         
00120         rospy.logerr('Param "action" is not set')
00121         sys.exit()
00122         
00123     if not rospy.has_param('~file'):
00124         
00125         rospy.logerr('Param "file" is not set')
00126         sys.exit()
00127 
00128 
00129     action = str(rospy.get_param('~action'))
00130     file = str(rospy.get_param('~file'))
00131     
00132     if action == "load":
00133         
00134         if not os.path.exists(file):
00135         
00136             rospy.logerr("File (%s) does not exist!",file)
00137             sys.exit()
00138         
00139         l.load(file)
00140         
00141     elif action == "save":
00142     
00143         l.save(file)
00144         
00145     else:
00146         
00147         rospy.logerr("Unknown action. Please use 'load' or 'save'.")
00148     
00149   except rospy.ROSInterruptException:
00150     pass


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