inhibitor.h
Go to the documentation of this file.
00001 /*
00002 Author: Minglong Li
00003 Affiliation: State Key Laboratory of High Performance Computing (HPCL)
00004              College of Computer, National University of Defense Technology
00005 Email: minglong_l@163.com
00006 Created on: June 4th, 2016
00007 */
00008 #ifndef INHIBITOR_H_
00009 #define INHIBITOR_H_
00010 
00011 #include <ros/ros.h>
00012 #include <nodelet/nodelet.h>
00013 #include <iostream>
00014 #include <pluginlib/class_list_macros.h>
00015 
00016 namespace micros_mars_task_alloc {
00017 
00018 template<typename UpperMsgType, typename LowerMsgType>
00019 class Inhibitor : public nodelet::Nodelet
00020 {
00021     //typedef typename boost::shared_ptr<MsgType> TMsgPtr;
00022     //typedef typename boost::shared_ptr<const T> TConstPtr;
00023 public:
00024     Inhibitor(){}
00025     virtual ~Inhibitor(){}
00026     virtual void onInit()
00027     {
00028         nh_ = getPrivateNodeHandle();
00029         std::cout << "Initialising nodelet inhibitor ..." << std::endl;        
00030         // resolve node(let) name
00031         /*
00032         std::string name = nh_.getUnresolvedNamespace();
00033         int pos = name.find_last_of('/');
00034         name = name.substr(pos + 1);
00035         std::cout << "Initialising nodelet... [" << name << "]"<< std::endl;
00036         */
00037         // get the parameters
00038         if (!(nh_.getParam("upper_topic", upper_topic_)))
00039         {
00040             std::cout << "Fail to get the parameter upper_topic." << std::endl;
00041             return;
00042         }
00043         if (!(nh_.getParam("lower_topic", lower_topic_)))
00044         {
00045             std::cout << "Fail to get the parameter lower_topic" << std::endl;
00046             return;
00047         }
00048         if (!(nh_.getParam("output_topic",output_topic_)))
00049         {
00050             std::cout << "Fail to get the parameter output_topic" << std::endl;
00051             return;
00052         }
00053         nh_.param<double>("time_duration", time_duration_, 1.0);
00054         std::cout << "inhibitor hello1" << std::endl;
00055         upper_msg_sub_ = nh_.subscribe(upper_topic_, 10, &Inhibitor::upperCB, this);
00056         std::cout << "inhibitor hello2" << std::endl;
00057         lower_msg_sub_ = nh_.subscribe(lower_topic_, 10, &Inhibitor::lowerCB, this);
00058         std::cout << "inhibitor hello3" << std::endl;
00059         pub_ = nh_.advertise<LowerMsgType>(output_topic_, 10);//the typename MsgType represents the type of the suppressed messages
00060         std::cout << "inhibitor was loaded successfully." << std::endl;
00061         
00062     }
00063     //void upperCB(boost::shared_ptr<MsgType const>& msg);
00064     void upperCB(const boost::shared_ptr<const UpperMsgType>& msg)
00065     {
00066         begin_ = ros::Time::now().toSec();
00067         /*
00068         pub_ = nh_.advertise<MsgType>(output_topic_, 1000);
00069         pub_.publish(msg);
00070         */
00071         std::cout << "The upper layer are working." << std::endl;
00072     };
00073 
00074     //void lowerCB(boost::shared_ptr<MsgType const>& msg);  
00075     void lowerCB(const boost::shared_ptr<const LowerMsgType>& msg)
00076     {
00077         end_ = ros::Time::now().toSec();
00078         if(end_-begin_ > time_duration_)
00079         {
00080             //pub_ = nh_.advertise<MsgType>(output_topic_, 1000);
00081             pub_.publish(msg);
00082             std::cout << "The upper layer has broken down, the lower layer messages output successfully." << std::endl;
00083         }
00084     };  
00085     
00086 private:
00087     ros::NodeHandle nh_;
00088     std::string upper_topic_, lower_topic_, output_topic_;
00089     ros::Subscriber upper_msg_sub_, lower_msg_sub_;
00090     ros::Publisher pub_;
00091     double begin_;//to calculate the time duration
00092     double end_;//to calculate the time duration
00093     double time_duration_;
00094 
00095 };
00096 
00097 }//namespace micros_mars_task_alloc
00098 #endif


micros_mars_task_alloc
Author(s): Minglong Li , Xiaodong Yi , Yanzhen Wang , Zhongxuan Cai
autogenerated on Mon Jul 1 2019 19:55:03