Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <std_msgs/Float32.h>
00003 #include <std_msgs/Bool.h>
00004
00005 ros::Publisher co2Pub;
00006 ros::Publisher detectedPub;
00007
00008 float avg;
00009
00010 float threshold;
00011
00012 float scale;
00013
00014 void co2_callback(const std_msgs::Float32::ConstPtr &msg){
00015 ROS_DEBUG("CO2 Callback");
00016 ROS_DEBUG("avg_old: %f", avg);
00017 avg = 0.9 * avg + 0.1 * msg->data;
00018
00019 ROS_DEBUG("avg: %f", avg);
00020 double value = fabs(msg->data - avg);
00021
00022 ROS_DEBUG("Value: %f", value);
00023 std_msgs::Float32 value_msg;
00024 value_msg.data = scale * value;
00025 co2Pub.publish(value_msg);
00026
00027 std_msgs::Bool bool_msg;
00028 bool_msg.data = value > threshold;
00029 detectedPub.publish(bool_msg);
00030
00031 }
00032
00033 int main(int argc, char **argv)
00034 {
00035 ros::init(argc, argv, "hector_barrel_detection");
00036
00037 ros::NodeHandle nh_;
00038
00039 avg = 0;
00040 threshold = 50;
00041 scale = 75.0;
00042
00043 ros::Subscriber co2Sub = nh_.subscribe("/co2raw", 100, &co2_callback);
00044
00045 co2Pub = nh_.advertise<std_msgs::Float32>("/co2", 100);
00046 detectedPub = nh_.advertise<std_msgs::Bool>("/co2detected", 100);
00047
00048 ros::spin();
00049 exit(0);
00050 }