priority_mux_base.h

Go to the documentation of this file.

#pragma once
 
// SPDX-License-Identifier: BSD-3-Clause
// SPDX-FileCopyrightText: Czech Technical University in Prague
 
#include <functional>
#include <map>
#include <string>
#include <unordered_map>
#include <utility>
 
#include <cras_cpp_common/optional.hpp>
#include <cras_cpp_common/log_utils.h>
#include <ros/duration.h>
#include <ros/time.h>
 
namespace cras
{
 
namespace priority_mux
{
 
struct TopicConfig
{
  ::std::string name;  
  ::std::string inTopic;  
  ::std::string outTopic;  
  int priority {0};  
  ::ros::Duration timeout;  
  size_t queueSize;  
};
 
struct LockConfig
{
  ::std::string name;  
  ::std::string topic;  
  int priority {0};  
  ::ros::Duration timeout;  
};
 
struct OutputTopicConfig
{
  ::std::string topic;  
  ::cras::optional<bool> forceLatch;  
  ::ros::WallDuration subscriberConnectDelay;  
  size_t numSubscribersToWait {0u};  
  size_t queueSize;  
};
}
 
class PriorityMux : public ::cras::HasLogger
{
public:
  typedef ::std::function<void(const ::std::string& name, const ::ros::Duration& timeout)> SetTimerFn;
 
  PriorityMux(const ::std::unordered_map<::std::string, ::cras::priority_mux::TopicConfig>& topicConfigs,
              const ::std::unordered_map<::std::string, ::cras::priority_mux::LockConfig>& lockConfigs,
              const SetTimerFn& setTimerFn, const ::ros::Time& now, const ::cras::LogHelperPtr& log,
              const ::std::string& noneTopic = "__none", int nonePriority = 0);
 
  virtual bool cb(const ::std::string& inTopic, const ::ros::Time& stamp, const ::ros::Time& now);
 
  virtual void disableCb(const ::std::string& inTopic, const ::ros::Time& stamp, bool disable, const ::ros::Time& now);
 
  virtual void lockCb(const ::std::string& topic, const ::ros::Time& stamp, bool locked, const ::ros::Time& now);
 
  virtual void update(const ::ros::Time& now);
 
  virtual void reset(const ::ros::Time& now);
 
  const ::std::unordered_map<::std::string, ::std::string>& getLastSelectedTopics() const;
 
  int getActivePriority() const;
 
  bool isDisabled(const ::std::string& inTopic, const ::ros::Time& now);
 
protected:
  int getHighestLockedPriority(const ::ros::Time& now);
 
  ::std::unordered_map<::std::string, ::cras::priority_mux::TopicConfig> topicConfigs;
 
  ::std::unordered_map<::std::string, ::cras::priority_mux::LockConfig> lockConfigs;
 
  SetTimerFn setTimer;
 
  /*
   * Implementation note: we use the composite key <priority, topic> instead of just <topic> in the maps below.
   * The reasoning is that in the ordered maps, sorting first by priority and only then by string comparison should be
   * much faster than sorting just by the strings.
   */
 
  ::std::map<::std::pair<int, ::std::string>, ::ros::Time> lastReceiveStamps;
 
  ::std::map<::std::pair<int, ::std::string>, ::ros::Time> disabledStamps;
 
  ::std::map<::std::pair<int, ::std::string>, ::ros::Time> lastLockStamps;
 
  ::std::map<::std::pair<int, ::std::string>, bool> lockStates;
 
  ::std::string noneTopic;
 
  int nonePriority;
 
  int lastActivePriority;
 
  ::std::unordered_map<::std::string, ::std::string> lastSelectedTopics;
 
private:
  void resetImpl(const ::ros::Time& now);
};
 
}