move_robot_sim_e0man1.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 import roslib; roslib.load_manifest('srs_user_tests')
00029 import rospy
00030 from std_srvs.srv import Empty as EmptySrv
00031 from geometry_msgs.msg import Pose
00032 from gazebo_msgs.srv import SetModelState
00033 from gazebo_msgs.msg import ModelState
00034 from geometry_msgs.msg import PoseWithCovarianceStamped
00035 from std_msgs.msg import Empty as EmptyMsg
00036 
00037 
00038 def main():
00039     rospy.init_node('move_robot_to_given_place')
00040     while not rospy.is_shutdown():
00041         rospy.sleep(60)
00042         print "reposition robot now"
00043         reposition_robot()
00044 
00045 def reposition_robot():    
00046     
00047     g_pause = rospy.ServiceProxy("/gazebo/pause_physics", EmptySrv)
00048     g_unpause = rospy.ServiceProxy("/gazebo/unpause_physics", EmptySrv)
00049     g_set_state = rospy.ServiceProxy("/gazebo/set_model_state",SetModelState)
00050     pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped,latch=True)
00051     #rospy.sleep(26)
00052     
00053     rospy.wait_for_service("/gazebo/pause_physics")
00054     
00055     rospy.loginfo("Pausing physics")
00056     
00057     try:
00058             
00059       g_pause()
00060             
00061     except Exception, e:
00062         
00063       rospy.logerr('Error on calling service: %s',str(e))
00064       
00065     
00066     pose = Pose()
00067     
00068     pose.position.x = rospy.get_param('~position_x')
00069     pose.position.y = rospy.get_param('~position_y')
00070     pose.position.z = rospy.get_param('~position_z')
00071     
00072     pose.orientation.x = rospy.get_param('~orientation_x')
00073     pose.orientation.y = rospy.get_param('~orientation_y')
00074     pose.orientation.z = rospy.get_param('~orientation_z')
00075     pose.orientation.w = rospy.get_param('~orientation_w')
00076     
00077     state = ModelState()
00078     
00079     state.model_name = "robot"
00080     state.pose = pose
00081     
00082     
00083     rospy.loginfo("Moving robot")
00084     try:
00085             
00086       ret = g_set_state(state)
00087       
00088       print ret.status_message
00089             
00090     except Exception, e:
00091         
00092       rospy.logerr('Error on calling service: %s',str(e))
00093     
00094     
00095       
00096       
00097     rospy.loginfo("Unpausing physics") 
00098       
00099     try:
00100             
00101       g_unpause()
00102             
00103     except Exception, e:
00104         
00105       rospy.logerr('Error on calling service: %s',str(e))
00106       
00107     
00108     
00109     
00110     loc = PoseWithCovarianceStamped()
00111     
00112     loc.pose.pose = pose
00113     loc.header.frame_id = "/map"
00114     #loc.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
00115     
00116     rospy.loginfo("Adjusting localization")
00117     pub.publish(loc)
00118     pub.publish(loc)
00119     pub.publish(loc)
00120         
00121 
00122 if __name__ == '__main__':
00123   try:
00124     main()
00125   except rospy.ROSInterruptException: pass


srs_user_tests
Author(s): SRS team
autogenerated on Sun Jan 5 2014 12:14:05