averaging_server.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <std_msgs/Float32.h>
00003 #include <actionlib/server/simple_action_server.h>
00004 #include <actionlib_tutorials/AveragingAction.h>
00005 
00006 class AveragingAction
00007 {
00008 public:
00009     
00010   AveragingAction(std::string name) : 
00011     as_(nh_, name, false),
00012     action_name_(name)
00013   {
00014     //register the goal and feeback callbacks
00015     as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
00016     as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));
00017 
00018     //subscribe to the data topic of interest
00019     sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
00020     as_.start();
00021   }
00022 
00023   ~AveragingAction(void)
00024   {
00025   }
00026 
00027   void goalCB()
00028   {
00029     // reset helper variables
00030     data_count_ = 0;
00031     sum_ = 0;
00032     sum_sq_ = 0;
00033     // accept the new goal
00034     goal_ = as_.acceptNewGoal()->samples;
00035   }
00036 
00037   void preemptCB()
00038   {
00039     ROS_INFO("%s: Preempted", action_name_.c_str());
00040     // set the action state to preempted
00041     as_.setPreempted();
00042   }
00043 
00044   void analysisCB(const std_msgs::Float32::ConstPtr& msg)
00045   {
00046     // make sure that the action hasn't been canceled
00047     if (!as_.isActive())
00048       return;
00049     
00050     data_count_++;
00051     feedback_.sample = data_count_;
00052     feedback_.data = msg->data;
00053     //compute the std_dev and mean of the data 
00054     sum_ += msg->data;
00055     feedback_.mean = sum_ / data_count_;
00056     sum_sq_ += pow(msg->data, 2);
00057     feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));
00058     as_.publishFeedback(feedback_);
00059 
00060     if(data_count_ > goal_) 
00061     {
00062       result_.mean = feedback_.mean;
00063       result_.std_dev = feedback_.std_dev;
00064 
00065       if(result_.mean < 5.0)
00066       {
00067         ROS_INFO("%s: Aborted", action_name_.c_str());
00068         //set the action state to aborted
00069         as_.setAborted(result_);
00070       }
00071       else 
00072       {
00073         ROS_INFO("%s: Succeeded", action_name_.c_str());
00074         // set the action state to succeeded
00075         as_.setSucceeded(result_);
00076       }
00077     } 
00078   }
00079 
00080 protected:
00081     
00082   ros::NodeHandle nh_;
00083   actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
00084   std::string action_name_;
00085   int data_count_, goal_;
00086   float sum_, sum_sq_;
00087   actionlib_tutorials::AveragingFeedback feedback_;
00088   actionlib_tutorials::AveragingResult result_;
00089   ros::Subscriber sub_;
00090 };
00091 
00092 int main(int argc, char** argv)
00093 {
00094   ros::init(argc, argv, "averaging");
00095 
00096   AveragingAction averaging(ros::this_node::getName());
00097   ros::spin();
00098 
00099   return 0;
00100 }


actionlib_tutorials
Author(s): Melonee Wise
autogenerated on Thu Jun 6 2019 21:11:25