localize_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/LocalizeAction.h>
00004 #include <std_srvs/Trigger.h>
00005 
00006 #include <nav2d_navigator/commands.h>
00007 
00008 typedef actionlib::SimpleActionClient<nav2d_navigator::LocalizeAction> LocalizeClient;
00009 
00010 LocalizeClient* gLocalizeClient;
00011 
00012 bool receiveCommand(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00013 {
00014         nav2d_navigator::LocalizeGoal goal;
00015         goal.velocity = 0.5;
00016         gLocalizeClient->sendGoal(goal);
00017         res.success = true;
00018         res.message = "Send LocalizeGoal to Navigator.";
00019         return true;
00020 }
00021 
00022 int main(int argc, char **argv)
00023 {
00024         ros::init(argc, argv, "Localize");
00025         ros::NodeHandle n;
00026         
00027         ros::ServiceServer cmdServer = n.advertiseService(NAV_LOCALIZE_SERVICE, &receiveCommand);
00028         gLocalizeClient = new LocalizeClient(NAV_LOCALIZE_ACTION, true);
00029         gLocalizeClient->waitForServer();
00030         
00031         ros::spin();
00032         
00033         delete gLocalizeClient;
00034         return 0;
00035 }


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