srs_user_tests: move_robot.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('srs_user_tests')
00004 import rospy
00005 from geometry_msgs.msg import PoseWithCovarianceStamped
00006 from geometry_msgs.msg import Pose
00007 
00008 
00009 def main():
00010     
00011     rospy.init_node('move_robot_to_given_place')
00012     
00013     pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped,latch=True)
00014     rospy.sleep(3)
00015 
00016     rospy.loginfo("Please move robot to start pose and press <Enter>.")
00017     raw_input()
00018     pose = Pose()
00019     
00020     pose.position.x = rospy.get_param('~position_x')
00021     pose.position.y = rospy.get_param('~position_y')
00022     pose.position.z = rospy.get_param('~position_z')
00023     
00024     pose.orientation.x = rospy.get_param('~orientation_x')
00025     pose.orientation.y = rospy.get_param('~orientation_y')
00026     pose.orientation.z = rospy.get_param('~orientation_z')
00027     pose.orientation.w = rospy.get_param('~orientation_w')
00028     
00029     loc = PoseWithCovarianceStamped()
00030 
00031     loc.pose.pose = pose
00032     loc.header.frame_id = "/map"
00033     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]
00034     pub.publish(loc)
00035     rospy.sleep(1)
00036     pub.publish(loc)
00037     rospy.sleep(1)
00038     pub.publish(loc)
00039 
00040 
00041 if __name__ == '__main__':
00042   try:
00043     main()
00044   except rospy.ROSInterruptException: pass