00001 #include <ros/ros.h> 00002 #include <move_base_msgs/MoveBaseAction.h> 00003 #include <actionlib/client/simple_action_client.h> 00004 00005 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; 00006 00007 int main(int argc, char** argv){ 00008 ros::init(argc, argv, "simple_navigation_goals"); 00009 00010 //tell the action client that we want to spin a thread by default 00011 MoveBaseClient ac("move_base", true); 00012 00013 //wait for the action server to come up 00014 while(!ac.waitForServer(ros::Duration(5.0))){ 00015 ROS_INFO("Waiting for the move_base action server to come up"); 00016 } 00017 00018 move_base_msgs::MoveBaseGoal goal; 00019 00020 //we'll send a goal to the robot to move 1 meter forward 00021 goal.target_pose.header.frame_id = "base_link"; 00022 goal.target_pose.header.stamp = ros::Time::now(); 00023 00024 goal.target_pose.pose.position.x = 1.0; 00025 goal.target_pose.pose.orientation.w = 1.0; 00026 00027 ROS_INFO("Sending goal"); 00028 ac.sendGoal(goal); 00029 00030 ac.waitForResult(); 00031 00032 if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 00033 ROS_INFO("Hooray, the base moved 1 meter forward"); 00034 else 00035 ROS_INFO("The base failed to move forward 1 meter for some reason"); 00036 00037 return 0; 00038 }