trial.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib
00004 roslib.load_manifest('room_explore')
00005 import rospy
00006 
00007 import tf
00008 
00009 from nav_msgs.srv import GetPlan, GetPlanRequest
00010 from geometry_msgs.msg import PoseStamped
00011 
00012 import util as ut
00013 import time
00014 
00015 #srv_name = '/move_base_node/NavfnROS/make_plan'
00016 srv_name = '/move_base_node/make_plan'
00017 
00018 rospy.init_node('trial_explore')
00019 listener = tf.TransformListener()
00020 listener.waitForTransform('/base_link', '/map',
00021                           rospy.Time(0), timeout = rospy.Duration(100) )
00022 
00023 get_plan = rospy.ServiceProxy( srv_name, GetPlan )
00024 rospy.get_param('/move_base_node/global_costmap/raytrace_range')
00025 rospy.get_param('/move_base_node/TrajectoryPlannerROS/xy_goal_tolerance')
00026 
00027 req = GetPlanRequest()
00028 req.tolerance = 0.5
00029 
00030 req.start = ut.robot_in_map( listener )
00031 
00032 req.goal.header.stamp = rospy.Time(0)
00033 req.goal.header.frame_id = '/map'
00034 req.goal.pose.position.x = 0.85
00035 #req.goal.pose.position.y = 0.85
00036 req.goal.pose.orientation.w = 1.0
00037 
00038 res = get_plan( req )
00039 found = bool( res.plan.poses != [] )
00040 print res
00041 print 'Found Path: ', found
00042 
00043 
00044 for i in xrange( 100 ):
00045     t0 = time.time()
00046     res = get_plan( req )
00047     t1 = time.time()
00048 
00049     print 'Rate: ', 1.0 / (t1 - t0)


rfid_explore_room
Author(s): Travis Deyle
autogenerated on Wed Nov 27 2013 12:04:36