topic_handle.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (CC BY-NC-SA 4.0 License)
00003  *
00004  *  Copyright (c) 2014, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  This work is licensed under the Creative Commons
00008  *  Attribution-NonCommercial-ShareAlike 4.0 International License.
00009  *
00010  *  To view a copy of this license, visit
00011  *  http://creativecommons.org/licenses/by-nc-sa/4.0/
00012  *  or send a letter to
00013  *  Creative Commons, 444 Castro Street, Suite 900,
00014  *  Mountain View, California, 94041, USA.
00015  *********************************************************************/
00016 
00017 /*
00018  * @author Enrique Fernandez
00019  * @author Siegfried Gevatter
00020  */
00021 
00022 #ifndef TOPIC_HANDLE_H
00023 #define TOPIC_HANDLE_H
00024 
00025 #include <ros/ros.h>
00026 #include <std_msgs/Bool.h>
00027 #include <geometry_msgs/Twist.h>
00028 
00029 #include <twist_mux/utils.h>
00030 #include <twist_mux/twist_mux.h>
00031 
00032 #include <boost/utility.hpp>
00033 #include <boost/scoped_ptr.hpp>
00034 
00035 #include <string>
00036 #include <vector>
00037 
00038 namespace twist_mux
00039 {
00040 
00041 template<typename T>
00042 class TopicHandle_ : boost::noncopyable
00043 {
00044 public:
00045 
00046   typedef int priority_type;
00047 
00058   TopicHandle_(ros::NodeHandle& nh, const std::string& name, const std::string& topic, double timeout, priority_type priority, TwistMux* mux)
00059     : nh_(nh)
00060     , name_(name)
00061     , topic_(topic)
00062     , timeout_(timeout)
00063     , priority_(clamp(priority, priority_type(0), priority_type(255)))
00064     , mux_(mux)
00065     , stamp_(0.0)
00066   {
00067     ROS_INFO_STREAM
00068     (
00069       "Topic handler '" << name_ << "' subscribed to topic '" << topic_ <<
00070       "': timeout = " << ((timeout_) ? std::to_string(timeout_) + "s" : "None") <<
00071       ", priority = " << static_cast<int>(priority_)
00072     );
00073   }
00074 
00075   virtual ~TopicHandle_()
00076   {
00077     subscriber_.shutdown();
00078   }
00079 
00086   bool hasExpired() const
00087   {
00088     return (timeout_ > 0.0) and
00089            ((ros::Time::now() - stamp_).toSec() > timeout_);
00090   }
00091 
00092   const std::string& getName() const
00093   {
00094     return name_;
00095   }
00096 
00097   const std::string& getTopic() const
00098   {
00099     return topic_;
00100   }
00101 
00102   const double& getTimeout() const
00103   {
00104     return timeout_;
00105   }
00106 
00111   const priority_type& getPriority() const
00112   {
00113     return priority_;
00114   }
00115 
00116   const T& getStamp() const
00117   {
00118     return stamp_;
00119   }
00120 
00121   const T& getMessage() const
00122   {
00123     return msg_;
00124   }
00125 
00126 protected:
00127   ros::NodeHandle nh_;
00128 
00129   std::string name_;
00130   std::string topic_;
00131   ros::Subscriber subscriber_;
00132   double timeout_;
00133   priority_type priority_;
00134 
00135 protected:
00136   TwistMux* mux_;
00137 
00138   ros::Time stamp_;
00139   T msg_;
00140 };
00141 
00142 class VelocityTopicHandle : public TopicHandle_<geometry_msgs::Twist>
00143 {
00144 private:
00145   typedef TopicHandle_<geometry_msgs::Twist> base_type;
00146 
00147 public:
00148   typedef typename base_type::priority_type priority_type;
00149 
00150   VelocityTopicHandle(ros::NodeHandle& nh, const std::string& name, const std::string& topic, double timeout, priority_type priority, TwistMux* mux)
00151     : base_type(nh, name, topic, timeout, priority, mux)
00152   {
00153     subscriber_ = nh_.subscribe(topic_, 1, &VelocityTopicHandle::callback, this);
00154   }
00155 
00156   bool isMasked(priority_type lock_priority) const
00157   {
00158     return hasExpired() or (getPriority() < lock_priority);
00159   }
00160 
00161   void callback(const geometry_msgs::TwistConstPtr& msg)
00162   {
00163     stamp_ = ros::Time::now();
00164     msg_   = *msg;
00165 
00166     // Check if this twist has priority.
00167     // Note that we have to check all the locks because they might time out
00168     // and since we have several topics we must look for the highest one in
00169     // all the topic list; so far there's no O(1) solution.
00170     if (mux_->hasPriority(*this))
00171     {
00172       mux_->publishTwist(msg);
00173     }
00174   }
00175 };
00176 
00177 class LockTopicHandle : public TopicHandle_<std_msgs::Bool>
00178 {
00179 private:
00180   typedef TopicHandle_<std_msgs::Bool> base_type;
00181 
00182 public:
00183   typedef typename base_type::priority_type priority_type;
00184 
00185   LockTopicHandle(ros::NodeHandle& nh, const std::string& name, const std::string& topic, double timeout, priority_type priority, TwistMux* mux)
00186     : base_type(nh, name, topic, timeout, priority, mux)
00187   {
00188     subscriber_ = nh_.subscribe(topic_, 1, &LockTopicHandle::callback, this);
00189   }
00190 
00195   bool isLocked() const
00196   {
00197     return hasExpired() or getMessage().data;
00198   }
00199 
00200   void callback(const std_msgs::BoolConstPtr& msg)
00201   {
00202     stamp_ = ros::Time::now();
00203     msg_   = *msg;
00204   }
00205 };
00206 
00207 } // namespace twist_mux
00208 
00209 #endif // TOPIC_HANDLE_H


twist_mux
Author(s): Enrique Fernandez , Siegfried-A. Gevatter Pujals
autogenerated on Sat Jun 8 2019 20:13:46