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 }
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
AveragingAction::sum_
float sum_
Definition: averaging_server.cpp:86
actionlib::SimpleActionServer::start
void start()
ros.h
AveragingAction::data_count_
int data_count_
Definition: averaging_server.cpp:85
simple_action_server.h
actionlib::SimpleActionServer::publishFeedback
void publishFeedback(const Feedback &feedback)
actionlib::SimpleActionServer::setSucceeded
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
AveragingAction::nh_
ros::NodeHandle nh_
Definition: averaging_server.cpp:82
AveragingAction::sum_sq_
float sum_sq_
Definition: averaging_server.cpp:86
AveragingAction::as_
actionlib::SimpleActionServer< actionlib_tutorials::AveragingAction > as_
Definition: averaging_server.cpp:83
AveragingAction::AveragingAction
AveragingAction(std::string name)
Definition: averaging_server.cpp:10
AveragingAction::goal_
int goal_
Definition: averaging_server.cpp:85
AveragingAction::goalCB
void goalCB()
Definition: averaging_server.cpp:27
AveragingAction::analysisCB
void analysisCB(const std_msgs::Float32::ConstPtr &msg)
Definition: averaging_server.cpp:44
actionlib::SimpleActionServer::isActive
bool isActive()
AveragingAction::result_
actionlib_tutorials::AveragingResult result_
Definition: averaging_server.cpp:88
AveragingAction::preemptCB
void preemptCB()
Definition: averaging_server.cpp:37
AveragingAction::sub_
ros::Subscriber sub_
Definition: averaging_server.cpp:89
actionlib::SimpleActionServer::registerPreemptCallback
void registerPreemptCallback(boost::function< void()> cb)
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
AveragingAction
Definition: averaging_server.cpp:6
actionlib::SimpleActionServer::setPreempted
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
AveragingAction::~AveragingAction
~AveragingAction(void)
Definition: averaging_server.cpp:23
actionlib::SimpleActionServer::registerGoalCallback
void registerGoalCallback(boost::function< void()> cb)
ros::this_node::getName
const ROSCPP_DECL std::string & getName()
actionlib::SimpleActionServer::setAborted
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
actionlib::SimpleActionServer::acceptNewGoal
boost::shared_ptr< const Goal > acceptNewGoal()
actionlib::SimpleActionServer< actionlib_tutorials::AveragingAction >
ros::spin
ROSCPP_DECL void spin()
AveragingAction::feedback_
actionlib_tutorials::AveragingFeedback feedback_
Definition: averaging_server.cpp:87
AveragingAction::action_name_
std::string action_name_
Definition: averaging_server.cpp:84
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle
ros::Subscriber
main
int main(int argc, char **argv)
Definition: averaging_server.cpp:92


actionlib_tutorials
Author(s): Melonee Wise
autogenerated on Wed Mar 2 2022 00:05:48