MockDriverNode.h

Go to the documentation of this file.

/*
 * Copyright (C) 2020-2021 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
#pragma once
 
#include <ros/ros.h>
#include <carma_utils/CARMAUtils.h>
#include "comm_types.h"
#include "ROSComms.h"
#include <vector>
 
namespace mock_drivers
{
struct PublisherWapper
{
  ros::Publisher pub;
  std::string base_topic_name;
};
 
class MockDriverNode
{
private:
  boost::shared_ptr<ros::CARMANodeHandle> cnh_;
  std::vector<PublisherWapper> publishers_;
  std::vector<ros::Subscriber> subscribers_;
  std::vector<ros::ServiceServer> services_;
 
  // used for testing
  bool dummy_ = false;  // if the mock driver node is a dummy then it wont initialize a CARMANodeHandle and its functions wont
                // do anything
  std::vector<std::string> topics_;
  std::vector<ros::Time> time_stamps_;
 
public:
 
  std::string getGraphName() const;
 
  template <typename T>
  void addPub(T comm)
  {
    if (!dummy_)
    {
      PublisherWapper pub;
      pub.pub = cnh_->advertise<decltype(comm->getTemplateType())>(comm->getTopic(), comm->getQueueSize());
      pub.base_topic_name = comm->getTopic();
      publishers_.push_back(pub);
    }
  }
 
  template <typename T>
  void addSub(T comm)
  {
    if (!dummy_)
    {
      subscribers_.push_back(cnh_->subscribe<decltype(comm->getTemplateType())>(
          comm->getTopic(), comm->getQueueSize(), &ROSComms<decltype(comm->getTemplateType())>::callback, comm));
    }
  }
 
  template <typename T>
  void addSrv(T comm)
  {
    if (!dummy_)
    {
      services_.push_back(cnh_->advertiseService(
          comm->getTopic(), &ROSComms<decltype(comm->getReqType()), decltype(comm->getResType())>::callback, comm));
    }
  }
 
  void spin(double rate) const;
 
  void setSpinCallback(std::function<bool()> cb) const;
 
  void init();
 
  template <typename T>
  void publishData(std::string topic, T msg)
  {
    if (!dummy_)
    {
      auto pub = std::find_if(publishers_.begin(), publishers_.end(),
                              [&](PublisherWapper p) { return p.base_topic_name == topic; });
      if (pub == publishers_.end())
        throw std::invalid_argument("Attempted to publish to topic " + topic + " but no publisher was found");
      pub->pub.publish(msg);
    }
    else
    {
      topics_.push_back(topic);
      time_stamps_.push_back(msg.header.stamp);
    }
  };
 
  template <typename T>  // TODO remove
  void publishDataNoHeader(std::string topic, T msg)
  {
    if (!dummy_)
    {
      auto pub = std::find_if(publishers_.begin(), publishers_.end(),
                              [&](PublisherWapper p) { return p.base_topic_name == topic; });
      if (pub == publishers_.end())
        throw std::invalid_argument("Attempted to publish to topic " + topic + " but no publisher was found");
      pub->pub.publish(msg);
    }
    else
    {
      topics_.push_back(topic);
    }
  };
 
  MockDriverNode();
  MockDriverNode(bool dummy);
 
  std::vector<std::string> getTopics()
  {
    return topics_;
  }
  std::vector<ros::Time> getTimeStamps()
  {
    return time_stamps_;
  }
  bool isDummy()
  {
    return dummy_;
  }
};
 
}  // namespace mock_drivers