set_goal_client.cpp
Go to the documentation of this file.
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 <nav2d_navigator/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 }


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:53:43