action_example.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/server/simple_action_server.h>
00015 #include <behavior_tree_core/BTAction.h>
00016 
00017 #include <string>
00018 
00019 
00020 enum Status {RUNNING, SUCCESS, FAILURE};  // BT return status
00021 
00022 
00023 class BTAction
00024 {
00025 protected:
00026     ros::NodeHandle nh_;
00027     // NodeHandle instance must be created before this line. Otherwise strange error may occur.
00028     actionlib::SimpleActionServer<behavior_tree_core::BTAction> as_;
00029     std::string action_name_;
00030     // create messages that are used to published feedback/result
00031     behavior_tree_core::BTFeedback feedback_;  // action feedback (SUCCESS, FAILURE)
00032     behavior_tree_core::BTResult result_;  // action feedback  (same as feedback for us)
00033 
00034 
00035 public:
00036     explicit BTAction(std::string name) :
00037         as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),
00038         action_name_(name)
00039     {
00040         // Starts the action server
00041         as_.start();
00042     }
00043 
00044     ~BTAction(void)
00045     {}
00046 
00047     void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
00048     {
00049         // publish info to the console for the user
00050         ROS_INFO("Starting Action");
00051 
00052         // start executing the action
00053         int i = 0;
00054         while (i < 5)
00055         {
00056             // check that preempt has not been requested by the client
00057             if (as_.isPreemptRequested())
00058             {
00059                 ROS_INFO("Action Halted");
00060 
00061                 // set the action state to preempted
00062                 as_.setPreempted();
00063                 break;
00064             }
00065             ROS_INFO("Executing Action");
00066 
00067             ros::Duration(0.5).sleep();  // waiting for 0.5 seconds
00068             i++;
00069         }
00070 
00071         if (i == 5)
00072         {
00073             set_status(SUCCESS);
00074         }
00075     }
00076 
00077 
00078     // returns the status to the client (Behavior Tree)
00079     void set_status(int status)
00080     {
00081         // Set The feedback and result of BT.action
00082         feedback_.status = status;
00083         result_.status = feedback_.status;
00084         // publish the feedback
00085         as_.publishFeedback(feedback_);
00086         // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE).
00087         as_.setSucceeded(result_);
00088 
00089         switch (status)  // Print for convenience
00090         {
00091         case SUCCESS:
00092             ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() );
00093             break;
00094         case FAILURE:
00095             ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() );
00096             break;
00097         default:
00098             break;
00099         }
00100     }
00101 };
00102 
00103 
00104 int main(int argc, char** argv)
00105 {
00106     ros::init(argc, argv, "action");
00107     ROS_INFO(" Enum: %d", RUNNING);
00108     ROS_INFO(" Action Ready for Ticks");
00109     BTAction bt_action(ros::this_node::getName());
00110     ros::spin();
00111 
00112     return 0;
00113 }


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