averaging_server.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <std_msgs/Float32.h>
4 #include <actionlib_tutorials/AveragingAction.h>
5 
7 {
8 public:
9 
10  AveragingAction(std::string name) :
11  as_(nh_, name, false),
12  action_name_(name)
13  {
14  //register the goal and feeback callbacks
17 
18  //subscribe to the data topic of interest
19  sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
20  as_.start();
21  }
22 
24  {
25  }
26 
27  void goalCB()
28  {
29  // reset helper variables
30  data_count_ = 0;
31  sum_ = 0;
32  sum_sq_ = 0;
33  // accept the new goal
34  goal_ = as_.acceptNewGoal()->samples;
35  }
36 
37  void preemptCB()
38  {
39  ROS_INFO("%s: Preempted", action_name_.c_str());
40  // set the action state to preempted
41  as_.setPreempted();
42  }
43 
44  void analysisCB(const std_msgs::Float32::ConstPtr& msg)
45  {
46  // make sure that the action hasn't been canceled
47  if (!as_.isActive())
48  return;
49 
50  data_count_++;
51  feedback_.sample = data_count_;
52  feedback_.data = msg->data;
53  //compute the std_dev and mean of the data
54  sum_ += msg->data;
55  feedback_.mean = sum_ / data_count_;
56  sum_sq_ += pow(msg->data, 2);
57  feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));
59 
60  if(data_count_ > goal_)
61  {
62  result_.mean = feedback_.mean;
63  result_.std_dev = feedback_.std_dev;
64 
65  if(result_.mean < 5.0)
66  {
67  ROS_INFO("%s: Aborted", action_name_.c_str());
68  //set the action state to aborted
70  }
71  else
72  {
73  ROS_INFO("%s: Succeeded", action_name_.c_str());
74  // set the action state to succeeded
76  }
77  }
78  }
79 
80 protected:
81 
84  std::string action_name_;
86  float sum_, sum_sq_;
87  actionlib_tutorials::AveragingFeedback feedback_;
88  actionlib_tutorials::AveragingResult result_;
90 };
91 
92 int main(int argc, char** argv)
93 {
94  ros::init(argc, argv, "averaging");
95 
97  ros::spin();
98 
99  return 0;
100 }
boost::shared_ptr< const Goal > acceptNewGoal()
void publishFeedback(const FeedbackConstPtr &feedback)
ros::Subscriber sub_
void analysisCB(const std_msgs::Float32::ConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
actionlib_tutorials::AveragingResult result_
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
#define ROS_INFO(...)
actionlib_tutorials::AveragingFeedback feedback_
void registerPreemptCallback(boost::function< void()> cb)
ROSCPP_DECL void spin()
AveragingAction(std::string name)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
std::string action_name_
int main(int argc, char **argv)
ros::NodeHandle nh_
actionlib::SimpleActionServer< actionlib_tutorials::AveragingAction > as_
void registerGoalCallback(boost::function< void()> cb)


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