2 # -*- coding: iso-8859-15 -*-
6 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
7 from actionlib_msgs.msg import GoalStatus
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")
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()
23 goal.target_pose.pose.position.x = 1.0
24 goal.target_pose.pose.orientation.w = 1.0
26 self.move_base.send_goal(goal)
28 self.move_base.wait_for_result()
30 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
31 rospy.loginfo("The base moved 1 meter forward")
33 rospy.logerr("The base failed to move forward 1 meter")
35 if __name__ == "__main__":