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