mapping_states.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 #   Copyright (c) 2010 \n
00007 #   Fraunhofer Institute for Manufacturing Engineering
00008 #   and Automation (IPA) \n\n
00009 #
00010 #################################################################
00011 #
00012 # \note
00013 #   Project name: care-o-bot
00014 # \note
00015 #   ROS stack name: cob_scenarios
00016 # \note
00017 #   ROS package name: cob_generic_states
00018 #
00019 # \author
00020 #   Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00021 #
00022 # \date Date of creation: Aug 2011
00023 #
00024 # \brief
00025 #   Implements generic states which can be used in multiple scenarios.
00026 #
00027 #################################################################
00028 #
00029 # Redistribution and use in source and binary forms, with or without
00030 # modification, are permitted provided that the following conditions are met:
00031 #
00032 #     - Redistributions of source code must retain the above copyright
00033 #       notice, this list of conditions and the following disclaimer. \n
00034 #     - Redistributions in binary form must reproduce the above copyright
00035 #       notice, this list of conditions and the following disclaimer in the
00036 #       documentation and/or other materials provided with the distribution. \n
00037 #     - Neither the name of the Fraunhofer Institute for Manufacturing
00038 #       Engineering and Automation (IPA) nor the names of its
00039 #       contributors may be used to endorse or promote products derived from
00040 #       this software without specific prior written permission. \n
00041 #
00042 # This program is free software: you can redistribute it and/or modify
00043 # it under the terms of the GNU Lesser General Public License LGPL as
00044 # published by the Free Software Foundation, either version 3 of the
00045 # License, or (at your option) any later version.
00046 #
00047 # This program is distributed in the hope that it will be useful,
00048 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00049 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00050 # GNU Lesser General Public License LGPL for more details.
00051 #
00052 # You should have received a copy of the GNU Lesser General Public
00053 # License LGPL along with this program.
00054 # If not, see <http://www.gnu.org/licenses/>.
00055 #
00056 #################################################################
00057 
00058 import roslib
00059 roslib.load_manifest('srs_states')
00060 import rospy
00061 import smach
00062 import smach_ros
00063 import actionlib
00064 import operator
00065 
00066 from cob_3d_mapping_msgs.msg import *
00067 from cob_srvs.srv import Trigger
00068 
00069 from shared_state_information import *
00070 
00071 from simple_script_server import *
00072 sss = simple_script_server()
00073 
00074 ## Initialize state
00075 #
00076 # This state will initialize all hardware drivers.
00077 class UpdateEnvMap(smach.State):
00078 
00079         def __init__(self):
00080 
00081                 smach.State.__init__(
00082                         self,
00083                         outcomes=['succeeded', 'failed', 'preempted'],
00084                         input_keys=['angle_range']) #good angle value: 0.4
00085                 self.client = actionlib.SimpleActionClient('trigger_mapping', TriggerMappingAction)
00086 
00087         def execute(self, userdata):
00088                 #scan_position = [-1.3, -1.0, 3.14]
00089                 #sss.move("torso","home")
00090                 #sss.move("head","front")
00091                 #sss.move("tray","down")
00092                 #sss.move("base",scan_position)
00093                 rospy.wait_for_service('registration/reset',10)
00094                 try:
00095                         reset_registration = rospy.ServiceProxy('registration/reset', Trigger)
00096                         resp1 = reset_registration()
00097                 except rospy.ServiceException, e:
00098                         print "Service call failed: %s"%e
00099                 rospy.wait_for_service('point_map/clear_map',10)
00100                 try:
00101                         clear_point_map = rospy.ServiceProxy('point_map/clear_map', Trigger)
00102                         resp1 = clear_point_map()
00103                 except rospy.ServiceException, e:
00104                         print "Service call failed: %s"%e
00105                 rospy.wait_for_service('geometry_map/clear_map',10)
00106                 try:
00107                         clear_geom_map = rospy.ServiceProxy('geometry_map/clear_map', Trigger)
00108                         resp1 = clear_geom_map()
00109                 except rospy.ServiceException, e:
00110                         print "Service call failed: %s"%e
00111                 goal = TriggerMappingGoal()
00112                 goal.start = True
00113                 if not self.client.wait_for_server():#rospy.Duration.from_sec(5.0)):
00114                         rospy.logerr('server not available')
00115                         return 'failed'
00116                 self.client.send_goal(goal)
00117                 if not self.client.wait_for_result():#rospy.Duration.from_sec(5.0)):
00118                         return 'failed'
00119                 sss.sleep(5.0)
00120                 angle_start = -userdata.angle_range/2
00121                 angle_stop = userdata.angle_range/2
00122                 sss.move("torso",[[-0.2,angle_start,-0.1]])
00123                 sss.move("torso",[[-0.2,angle_stop,-0.1]])
00124                 sss.move("torso",[[0,angle_stop,0]])
00125                 sss.move("torso",[[0,angle_start,0]])
00126                 goal.start = False
00127                 self.client.send_goal(goal)
00128                 self.client.wait_for_result(rospy.Duration.from_sec(5.0))
00129                 sss.move("torso","home")
00130                 #move neck/base
00131                 #get map
00132 
00133                 return 'succeeded'
00134 
00135 """experimental state, should be replaces by generic state for approach pose"""
00136 class ApproachScanPose(smach.State):
00137 
00138     def __init__(self):
00139 
00140         smach.State.__init__(
00141             self,
00142             outcomes=['succeeded', 'failed'],
00143             input_keys=['scan_pose']) #good angle value: 0.4
00144 
00145     def execute(self, userdata):
00146         scan_pose = userdata.scan_pose #[-1.3, -1.0, 3.14]
00147         sss.move("torso","home")
00148         sss.move("head","front")
00149         #sss.move("tray","down")
00150         #sss.move("base",scan_pose)
00151 
00152         return 'succeeded'
00153 
00154 class Map360(smach.State):
00155 
00156         def __init__(self):
00157 
00158                 smach.State.__init__(
00159                         self,
00160                         outcomes=['succeeded', 'failed'],
00161                         input_keys=['angle_range']) #good angle value: 0.4
00162                 self.client = actionlib.SimpleActionClient('trigger_mapping', TriggerMappingAction)
00163 
00164         def execute(self, userdata):
00165                 scan_pose = [0, 0, 0]
00166                 sss.move("base",scan_pose)
00167                 sss.move("torso","home")
00168                 sss.move("head","front")
00169                 sss.move("tray","down")
00170                 #sss.move("base",scan_position)
00171                 goal = TriggerMappingGoal()
00172                 goal.start = True
00173                 if not self.client.wait_for_server():#rospy.Duration.from_sec(5.0)):
00174                         rospy.logerr('server not available')
00175                         return 'failed'
00176                 self.client.send_goal(goal)
00177                 if not self.client.wait_for_result():#rospy.Duration.from_sec(5.0)):
00178                         return 'failed'
00179                 i = 0.2
00180                 ctr = 0
00181                 while i <= 6.2:
00182                         scan_pose[2]=i
00183                         sss.move("base",scan_pose)
00184                         if operator.mod(ctr,2) == 0:
00185                                 sss.move("torso",[[-0.2,0.0,-0.2]])
00186                         else:
00187                                 sss.move("torso","home")
00188                         #sss.sleep(0.5)
00189                         i = i+0.2
00190                         ctr = ctr+1
00191                 goal.start = False
00192                 self.client.send_goal(goal)
00193                 self.client.wait_for_result(rospy.Duration.from_sec(5.0))
00194                 #sss.move("torso","home")
00195                 #move neck/base
00196                 #get map
00197 
00198                 return 'succeeded'


srs_states
Author(s): Georg Arbeiter
autogenerated on Mon Oct 6 2014 08:34:06