fibonacci_server.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
3 #include <actionlib_tutorials/FibonacciAction.h>
4 
6 {
7 protected:
8 
10  actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
11  std::string action_name_;
12  // create messages that are used to published feedback/result
13  actionlib_tutorials::FibonacciFeedback feedback_;
14  actionlib_tutorials::FibonacciResult result_;
15 
16 public:
17 
18  FibonacciAction(std::string name) :
19  as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
20  action_name_(name)
21  {
22  as_.start();
23  }
24 
26  {
27  }
28 
29  void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
30  {
31  // helper variables
32  ros::Rate r(1);
33  bool success = true;
34 
35  // push_back the seeds for the fibonacci sequence
36  feedback_.sequence.clear();
37  feedback_.sequence.push_back(0);
38  feedback_.sequence.push_back(1);
39 
40  // publish info to the console for the user
41  ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
42 
43  // start executing the action
44  for(int i=1; i<=goal->order; i++)
45  {
46  // check that preempt has not been requested by the client
47  if (as_.isPreemptRequested() || !ros::ok())
48  {
49  ROS_INFO("%s: Preempted", action_name_.c_str());
50  // set the action state to preempted
51  as_.setPreempted();
52  success = false;
53  break;
54  }
55  feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
56  // publish the feedback
57  as_.publishFeedback(feedback_);
58  // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
59  r.sleep();
60  }
61 
62  if(success)
63  {
64  result_.sequence = feedback_.sequence;
65  ROS_INFO("%s: Succeeded", action_name_.c_str());
66  // set the action state to succeeded
67  as_.setSucceeded(result_);
68  }
69  }
70 
71 
72 };
73 
74 
75 int main(int argc, char** argv)
76 {
77  ros::init(argc, argv, "fibonacci");
78 
79  FibonacciAction fibonacci("fibonacci");
80  ros::spin();
81 
82  return 0;
83 }
void publishFeedback(const FeedbackConstPtr &feedback)
std::string action_name_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
int main(int argc, char **argv)
actionlib::SimpleActionServer< actionlib_tutorials::FibonacciAction > as_
ros::NodeHandle nh_
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
ROSCPP_DECL void spin()
void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
bool sleep()
actionlib_tutorials::FibonacciResult result_
FibonacciAction(std::string name)
actionlib_tutorials::FibonacciFeedback feedback_


actionlib_tutorials
Author(s): Melonee Wise
autogenerated on Wed Jun 5 2019 20:54:23