Program Listing for File service_wrapper.hpp

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

#ifndef RIG_RECONFIGURE_SERVICE_WRAPPER_HPP
#define RIG_RECONFIGURE_SERVICE_WRAPPER_HPP

#include <utility>
#include <vector>
#include <string>
#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include "queue.hpp"
#include "requests.hpp"
#include "responses.hpp"

struct FutureTimeoutContainer {
    FutureTimeoutContainer() {
        timeSent = std::chrono::system_clock::now();
    }

    std::chrono::time_point<std::chrono::system_clock> timeSent;
    bool handled = false;
    bool timeoutReached = false;
};

class ServiceWrapper {
  public:
    explicit ServiceWrapper(bool ignoreDefaultParameters_ = true);

    void terminate();

    void setNodeOfInterest(const std::string &name);

    void pushRequest(RequestPtr &&request);

    ResponsePtr tryPopResponse();

    void checkForTimeouts();

    void setIgnoreDefaultParameters(bool ignoreDefaultParameters);

  private:
    void threadFunc();
    void handleRequest(const RequestPtr &request);

    Queue<RequestPtr> requestQueue;
    Queue<ResponsePtr> responseQueue;

    std::atomic_bool terminateThread = false;

    bool ignoreDefaultParameters;
    std::string nodeName;
    std::thread thread;
    std::thread rosThread;
    std::promise<bool> terminationHelper;

    std::mutex unfinishedROSRequestsMutex;
    std::vector<std::shared_ptr<FutureTimeoutContainer>> unfinishedROSRequests;

    std::shared_ptr<rclcpp::Node> node;
    rclcpp::executors::SingleThreadedExecutor executor;

    // clients for calling the different services
    rclcpp::Client<rcl_interfaces::srv::ListParameters>::SharedPtr listParametersClient;
    rclcpp::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr describeParametersClient;
    rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr getParametersClient;
    rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr setParametersClient;

    // callbacks for the results of the futures
    void nodeParametersReceived(const rclcpp::Client<rcl_interfaces::srv::ListParameters>::SharedFuture &future,
                                const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer);
    void parameterDescriptionsReceived(const rclcpp::Client<rcl_interfaces::srv::DescribeParameters>::SharedFuture &future,
                                       const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer);
    void parameterValuesReceived(const rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedFuture &future,
                                 const std::vector<rcl_interfaces::msg::ParameterDescriptor> &parameterDescriptors,
                                 const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer);
    void parameterModificationResponseReceived(const rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture &future,
                                               const std::string &parameterName,
                                               const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer);
};

#endif // RIG_RECONFIGURE_SERVICE_WRAPPER_HPP