00001 #include <ros/ros.h> 00002 #include <actionlib/client/simple_action_client.h> 00003 #include <robot_navigator/MoveToPosition2DAction.h> 00004 #include <geometry_msgs/PoseStamped.h> 00005 #include <tf/transform_datatypes.h> 00006 #include <robot_navigator/commands.h> 00007 00008 typedef actionlib::SimpleActionClient<robot_navigator::MoveToPosition2DAction> MoveClient; 00009 00010 MoveClient* gMoveClient; 00011 00012 void receiveGoal(const geometry_msgs::PoseStamped::ConstPtr& msg) 00013 { 00014 robot_navigator::MoveToPosition2DGoal goal; 00015 goal.target_pose.x = msg->pose.position.x; 00016 goal.target_pose.y = msg->pose.position.y; 00017 goal.target_pose.theta = tf::getYaw(msg->pose.orientation); 00018 goal.target_distance = 0.25; 00019 goal.target_angle = 0.1; 00020 00021 gMoveClient->sendGoal(goal); 00022 } 00023 00024 int main(int argc, char **argv) 00025 { 00026 ros::init(argc, argv, "SetGoal"); 00027 ros::NodeHandle n; 00028 00029 ros::Subscriber goalSubscriber = n.subscribe("goal", 1, &receiveGoal); 00030 gMoveClient = new MoveClient(NAV_MOVE_ACTION, true); 00031 gMoveClient->waitForServer(); 00032 00033 ros::spin(); 00034 00035 delete gMoveClient; 00036 return 0; 00037 }