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__SOAR_RUNNER_HPP_
#define SOAR_ROS__SOAR_RUNNER_HPP_

#include <atomic>
#include <chrono>
#include <cstdlib>
#include <ctime>
#include <map>
#include <memory>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>
#include "sml_Client.h"

#include "SoarAgent.hpp"

namespace soar_ros
{

    void SoarPrintEventHandler(
        [[maybe_unused]] sml::smlPrintEventId id,
        void *pSoarRunner_,
        sml::Agent *pAgent,
        char const *pMessage);

    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
    {
    public:
        explicit SoarRunner(const std::string &node_name);
        ~SoarRunner();

        std::shared_ptr<SoarAgent> addAgent(
            const std::string &agent_name,
            const std::string &source_file);

        void run();

        void startThread()
        {
            if (m_running.load())
            {
                RCLCPP_WARN(get_logger(), "runThread already running");
                return;
            }
            m_running.store(true);
            RCLCPP_INFO(get_logger(), "Starting runThread");
            m_soar_thread = std::thread(&SoarRunner::soarRunLoop, this);
        }

        void stopThread()
        {
            if (!m_running.load())
            {
                RCLCPP_DEBUG(get_logger(), "Run thread already stopped!");
                return;
            }
            m_running.store(false);
            RCLCPP_WARN(get_logger(), "Stopping runThread");
            if (m_soar_thread.joinable())
            {
                RCLCPP_DEBUG(get_logger(), "Waiting for run thread to join");
                m_soar_thread.join();
            }
        }

        void updateWorld()
        {
            for (auto &agent : m_agents)
            {
                agent->updateWorld();
            }
        }

        // ---- ROS2 service handlers -------------------------------------------

        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);

    private:
        sml::Kernel *m_kernel{nullptr};
        std::vector<std::shared_ptr<SoarAgent>> m_agents;

        std::atomic<bool> m_running{false};
        std::thread m_soar_thread;
        bool m_debug{false};

        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;

        void soarRunLoop();
    };

} // namespace soar_ros

#endif // SOAR_ROS__SOAR_RUNNER_HPP_