Program Listing for File SoarRunner.hpp

↰ Return to documentation for file (include/soar_ros/SoarRunner.hpp)

// Copyright 2024 Moritz Schmidt
//
// 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.

#ifndef SOAR_ROS__SOARRUNNER_HPP_
#define SOAR_ROS__SOARRUNNER_HPP_

#include <cstdlib>
#include <iostream>
#include <map>
#include <memory>
#include <queue>
#include <sml_Client.h>
#include <stdexcept>
#include <string>
#include <thread>
#include <vector>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_srvs/srv/trigger.hpp>

#include "Client.hpp"
#include "Interface.hpp"
#include "Publisher.hpp"
#include "Service.hpp"
#include "Subscriber.hpp"
#include "ActionClient.hpp"

namespace soar_ros
{
void SoarPrintEventHandler([[maybe_unused]] sml::smlPrintEventId id, void * pSoarRunner_,
  sml::Agent * pAgent,
  char const * pMessagee);

void updateEventHandler([[maybe_unused]] sml::smlUpdateEventId id, void * pSoarRunner_,
  [[maybe_unused]] sml::Kernel * kernel_ptr, [[maybe_unused]] sml::smlRunFlags run_flags);

class SoarRunner : public rclcpp::Node
{
private:
  std::vector<sml::Agent *> agents;

  sml::Kernel * pKernel;

  std::thread runThread;

  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_getSoarKernelStatus;

  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_kernelRun;

  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_kernelStop;

  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_debuggerLaunch;

  std::map<sml::Agent *,
    std::map<std::string, std::shared_ptr<soar_ros::OutputBase>>> outputs_by_agent;

  std::map<sml::Agent *, std::vector<std::shared_ptr<soar_ros::InputBase>>> inputs_by_agent;

  bool m_debug;

  std::map<sml::Agent *, sml::Identifier *> output_links;

  void processOutputLinkChanges(sml::Agent * agent);

  void processInput(sml::Agent * agent);

  void run()
  {
    while (isRunning.load()) {
      pKernel->RunAllAgents(1);
    }
  }

  std::string getSoarLogFilePath();

  std::atomic<bool> isRunning;

public:
  SoarRunner();

  void debuggerLaunch([[maybe_unused]] const std::shared_ptr<rmw_request_id_t> request_header,
    [[maybe_unused]] std::shared_ptr<std_srvs::srv::Trigger::Request> request,
    std::shared_ptr<std_srvs::srv::Trigger::Response> response);

  void getSoarKernelStatus([[maybe_unused]] const std::shared_ptr<rmw_request_id_t> request_header,
    [[maybe_unused]] std::shared_ptr<std_srvs::srv::Trigger::Request> request,
    std::shared_ptr<std_srvs::srv::Trigger::Response> response);

  void runSoarKernel([[maybe_unused]] const std::shared_ptr<rmw_request_id_t> request_header,
    [[maybe_unused]] std::shared_ptr<std_srvs::srv::Trigger::Request> request,
    std::shared_ptr<std_srvs::srv::Trigger::Response> response);

  void stopSoarKernel([[maybe_unused]] const std::shared_ptr<rmw_request_id_t> request_header,
    [[maybe_unused]] std::shared_ptr<std_srvs::srv::Trigger::Request> request,
    std::shared_ptr<std_srvs::srv::Trigger::Response> response);

  sml::Agent * addAgent(const std::string & agent_name, const std::string & path_productions);

  ~SoarRunner();

  const std::vector<sml::Agent *> & getAgents() const {return agents;}

  template<typename T>
  bool addPublisher(std::shared_ptr<Publisher<T>> output)
  {
    auto agent = output.get()->getAgent();
    outputs_by_agent[agent][output.get()->getTopic()] = output;
    return true;
  }

  template<typename T>
  bool addPublisher(std::shared_ptr<Publisher<T>> output, const std::string & commandName)
  {
    auto agent = output.get()->getAgent();
    outputs_by_agent[agent][commandName] = output;
    return true;
  }

  template<typename T>
  bool addSubscriber(std::shared_ptr<Subscriber<T>> input)
  {
    auto agent = input.get()->getAgent();
    inputs_by_agent[agent].push_back(input);
    return true;
  }

  template<typename T>
  bool addService(std::shared_ptr<Service<T>> service)
  {
    return addService(service, service.get()->getTopic());
  }

  template<typename T>
  bool addService(std::shared_ptr<Service<T>> service, const std::string & commandName)
  {
    auto agent = service.get()->getAgent();
    outputs_by_agent[agent][commandName] = std::static_pointer_cast<soar_ros::OutputBase>(service);
    inputs_by_agent[agent].push_back(std::static_pointer_cast<soar_ros::InputBase>(service));
    return true;
  }

  template<typename T>
  bool addClient(std::shared_ptr<Client<T>> client)
  {
    return addClient(client, client.get()->getTopic());
  }

  template<typename T>
  bool addClient(std::shared_ptr<Client<T>> client, const std::string & commandName)
  {
    auto agent = client.get()->getAgent();
    outputs_by_agent[agent][commandName] = std::static_pointer_cast<soar_ros::OutputBase>(client);
    inputs_by_agent[agent].push_back(std::static_pointer_cast<soar_ros::InputBase>(client));
    return true;
  }

  template<typename T>
  bool addActionClient(std::shared_ptr<ActionClient<T>> action_client)
  {
    return addActionClient(action_client, action_client.get()->getTopic());
  }

  template<typename T>
  bool addActionClient(
    std::shared_ptr<ActionClient<T>> action_client,
    const std::string & commandName)
  {
    auto agent = action_client.get()->getAgent();
    outputs_by_agent[agent][commandName] =
      std::static_pointer_cast<soar_ros::Output<typename T::Goal::SharedPtr>>(action_client);
    inputs_by_agent[agent].push_back(
        std::static_pointer_cast<soar_ros::Input<typename T::Feedback::SharedPtr>>(action_client));
    inputs_by_agent[agent].push_back(
        std::static_pointer_cast<soar_ros::Input<typename rclcpp_action::ClientGoalHandle<T>::
      WrappedResult>>(
            action_client));
    inputs_by_agent[agent].push_back(std::static_pointer_cast<soar_ros::Input<bool>>(
        action_client));
    return true;
  }

  void startThread()
  {
    // If thread is already running, skip
    if (isRunning.load() == true) {
      RCLCPP_WARN(this->get_logger(), "runThread already running");
      return;
    } else {
      isRunning.store(true);
    }

    RCLCPP_INFO(this->get_logger(), "Starting runThread");
    runThread = std::thread(&SoarRunner::run, this);
  }

  void stopThread()
  {
    if (isRunning.load() == false) {
      RCLCPP_INFO(this->get_logger(), "Run thread already stopped!");
      return;
    }

    isRunning.store(false);
    RCLCPP_WARN(this->get_logger(), "Stopping runThread");

    if (runThread.joinable()) {
      RCLCPP_INFO(this->get_logger(), "Waiting for run thread to join");
      runThread.join();
    }
  }

  void updateWorld()
  {
    for (auto * agent : agents) {
      if (agent == nullptr) {
        continue;
      }

      // Read Soar output
      processOutputLinkChanges(agent);

      if (output_links[agent] == nullptr) {
        output_links[agent] = agent->GetOutputLink();
        RCLCPP_DEBUG(this->get_logger(), "Output link reference invalid!");
      }

      // Write to Soar input-link
      processInput(agent);
    }
  }
};  // class SoarRunner

}  // namespace soar_ros

#endif  // SOAR_ROS__SOARRUNNER_HPP_