scripts/square.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import rospy
   5 import tf
   6 import actionlib
   7 from math import *
   8 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
   9 from actionlib_msgs.msg import GoalStatus
  10 from optparse import OptionParser
  11
  12
  13 class Square:
  14         def __init__(self):
  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")
  19
  20         def next_pos(self, x, y, angle):
  21                 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
  22
  23                 rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))
  24
  25                 goal = MoveBaseGoal()
  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)
  35
  36                 self.move_base.wait_for_result()
  37
  38                 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
  39                         rospy.loginfo("The base moved to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
  40                 else:
  41                         rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
  42                         raise
  43
  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)
  49
  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()
  54
  55         p = Square()
  56         p.run(float(options.length))