2 # -*- coding: iso-8859-15 -*-
8 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
9 from actionlib_msgs.msg import GoalStatus
10 from optparse import OptionParser
15 rospy.init_node('umbmark')
16 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
17 while not self.move_base.wait_for_server(rospy.Duration(5)):
18 rospy.loginfo("Waiting for the move_base action server to come up")
20 def next_pos(self, x, y, angle):
21 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
23 rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))
26 goal.target_pose.header.frame_id = "odom"
27 goal.target_pose.header.stamp = rospy.Time.now()
28 goal.target_pose.pose.position.x = x
29 goal.target_pose.pose.position.y = y
30 goal.target_pose.pose.orientation.x = odom_quat[0]
31 goal.target_pose.pose.orientation.y = odom_quat[1]
32 goal.target_pose.pose.orientation.z = odom_quat[2]
33 goal.target_pose.pose.orientation.w = odom_quat[3]
34 self.move_base.send_goal(goal)
36 self.move_base.wait_for_result()
38 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
39 rospy.loginfo("The base moved to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
41 rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
44 def run(self, size=1):
45 self.next_pos(0, 0, 0)
46 self.next_pos(size, 0, 0)
47 self.next_pos(size, -size, 0)
48 self.next_pos(0, size, 0)
50 if __name__ == "__main__":
51 parser = OptionParser()
52 parser.add_option("-l", "--length", dest="length", default=2, help="Square size")
53 (options, args) = parser.parse_args()
56 p.run(float(options.length))