action_client.cpp
Go to the documentation of this file.
00001 /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
00002 *
00003 *   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
00004 *   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
00005 *   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
00006 *   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
00007 *
00008 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00009 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
00010 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00011 */
00012 
00013 #include <ros/ros.h>
00014 #include <actionlib/client/simple_action_client.h>
00015 #include <actionlib/client/terminal_state.h>
00016 #include <behavior_tree_core/BTAction.h>
00017 
00018 
00019 int main(int argc, char **argv)
00020 {
00021     ros::init(argc, argv, "test_action");
00022 
00023     // create the action client
00024     // true causes the client to spin its own thread
00025     actionlib::SimpleActionClient<behavior_tree_core::BTAction> ac("action", true);
00026     behavior_tree_core::BTResult node_result;
00027     ROS_INFO("Waiting for action server to start.");
00028     // wait for the action server to start
00029     ac.waitForServer();  // will wait for infinite time
00030 
00031     behavior_tree_core::BTGoal goal;
00032 
00033 
00034     int command = 0;
00035     bool isRunning = false;
00036 
00037 
00038     while (command != 3)
00039     {
00040         ROS_INFO("Send a command: 1:start the action | 2:stop the action | 3:exit the program");
00041         std::cin >> command;
00042 
00043         switch (command)
00044         {
00045         case 1:
00046             if (!isRunning)
00047             {
00048                 ROS_INFO("I am running the request");
00049                 ac.sendGoal(goal);
00050                 isRunning = true;
00051                 node_result = *(ac.getResult());
00052                 ROS_INFO("Action finished, status: %d", node_result.status);
00053             }
00054             else
00055             {
00056                 ROS_INFO("I am re-running the request");
00057                 ac.cancelGoal();
00058                 ac.sendGoal(goal);
00059                 ROS_INFO("Action finished, status: %d", node_result.status);
00060             }
00061             break;
00062         case 2:
00063             ROS_INFO("I am cancelling the request");
00064             ac.cancelGoal();
00065             isRunning = false;
00066             break;
00067         default:
00068             break;
00069         }
00070     }
00071     return 0;
00072 }
00073 


behavior_tree_leaves
Author(s): Michele Colledanchise
autogenerated on Thu Jun 6 2019 18:19:15