Go to the documentation of this file.00001
00002 import rospy
00003 from actionlib import SimpleActionClient, GoalStatus
00004 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00005
00006
00007 def main():
00008 rospy.init_node("simple_navigation_goals")
00009 move_base_client = SimpleActionClient('move_base', MoveBaseAction)
00010 rospy.loginfo('Connecting to server')
00011 move_base_client.wait_for_server()
00012
00013 goal = MoveBaseGoal()
00014
00015 goal.target_pose.header.frame_id = 'komodo_1/base_link'
00016 goal.target_pose.header.stamp = rospy.Time.now()
00017 goal.target_pose.pose.position.x = -1.0
00018 goal.target_pose.pose.orientation.w = 1.0
00019
00020 rospy.loginfo('Sending goal')
00021 move_base_client.send_goal(goal)
00022 move_base_client.wait_for_result()
00023
00024 if move_base_client.get_state() == GoalStatus.SUCCEEDED:
00025 rospy.loginfo('Hooray, the base moved 1 meter forward')
00026 else:
00027 rospy.loginfo('The base failed to move forward 1 meter for some reason')
00028
00029
00030 if __name__ == '__main__':
00031 main()