scripts/waypoint.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import rospy
   5 import actionlib
   6 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
   7 from actionlib_msgs.msg import GoalStatus
   8
   9 class Waypoint:
  10         def __init__(self):
  11                 rospy.init_node('waypoint')
  12                 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
  13                 while not self.move_base.wait_for_server(rospy.Duration(5)):
  14                         rospy.loginfo("Waiting for the move_base action server to come up")
  15
  16         def send_goal(self):
  17                 goal = MoveBaseGoal()
  18
  19                 # we'll send a goal to the robot to move 1 meter forward
  20                 goal.target_pose.header.frame_id = "base_footprint"
  21                 goal.target_pose.header.stamp = rospy.Time.now()
  22
  23                 goal.target_pose.pose.position.x = 1.0
  24                 goal.target_pose.pose.orientation.w = 1.0
  25
  26                 self.move_base.send_goal(goal)
  27
  28                 self.move_base.wait_for_result()
  29
  30                 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
  31                         rospy.loginfo("The base moved 1 meter forward")
  32                 else:
  33                         rospy.logerr("The base failed to move forward 1 meter")
  34
  35 if __name__ == "__main__":
  36         p = Waypoint()
  37         p.send_goal()