Services and Actions API

// simplified
#include <horus/service.hpp>
#include <horus/action.hpp>

#include <horus/service.hpp>
#include <chrono>

using namespace std::chrono_literals;

horus::ServiceClient client("add_two_ints");

// Check that the handle was created successfully
if (!client) {
    fprintf(stderr, "Failed to create service client\n");
    return;
}

// Send request, wait up to 1 second for response
auto response = client.call(R"({"a": 3, "b": 4})", 1000ms);

if (response) {
    printf("Response: %s\n", response->c_str());
    // Output: {"sum": 7}
} else {
    printf("Service call timed out or failed\n");
}

using namespace std::chrono_literals;   // for the `ms` suffix
// (HORUS's own suffix is `500_ms`, from `using namespace horus::literals;`)

// String literal
auto r1 = client.call(R"({"x": 1.0})", 500ms);

// std::string
std::string request = R"({"x": 1.0, "y": 2.0})";
auto r2 = client.call(request, 500ms);

#include <horus/service.hpp>
#include <atomic>
#include <chrono>
#include <cstring>
#include <cstdio>
#include <thread>

std::atomic<bool> running{true};  // cleared from your shutdown path

horus::ServiceServer server("add_two_ints");

server.set_handler([](const uint8_t* req, size_t req_len,
                      uint8_t* res, size_t* res_len) -> bool {
    // Parse request (in production, use a JSON library)
    // For this example, assume req is: {"a": 3, "b": 4}
    int a = 3, b = 4;  // parsed from req

    // Write response. *res_len arrives holding the buffer capacity -- never
    // hard-code a size here.
    int written = snprintf(reinterpret_cast<char*>(res), *res_len,
                           R"({"sum": %d})", a + b);
    if (written < 0 || static_cast<size_t>(written) >= *res_len) return false;
    *res_len = static_cast<size_t>(written);
    return true;  // true = success, false = error
});

// Drive the server so it answers requests. Call in a loop on the constructing
// thread (or from a scheduled node tick).
while (running) {
    server.process();
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

using Handler = bool(*)(const uint8_t* req, size_t req_len,
                        uint8_t* res, size_t* res_len);

#include <horus/action.hpp>
#include <chrono>
#include <cstdio>
#include <string>
#include <thread>

horus::ActionClient client("navigate_to_pose");
if (!client) return;

auto goal = client.send_goal(R"({"target_x": 5.0, "target_y": 3.0})");
if (!goal) return;

printf("Goal %lu submitted\n", goal.id());

// Drain feedback and poll the result topic until the goal reaches a terminal status
horus::GoalStatus status = horus::GoalStatus::Pending;
std::string result_json;
uint64_t fb_id = 0;
std::string fb_json;
auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(30);
while (std::chrono::steady_clock::now() < deadline) {
    // One feedback topic per client, not per goal -- fb_id says which goal it is
    while (client.poll_feedback(fb_id, fb_json)) {
        printf("goal %lu feedback: %s\n", fb_id, fb_json.c_str());
    }
    if (client.poll_result(goal.id(), status, result_json)) break;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

if (status == horus::GoalStatus::Succeeded) {
    printf("Navigation complete: %s\n", result_json.c_str());
}

auto goal = client.send_goal(R"({"x": 10.0, "y": 0.0})");
// Cancel after 5 seconds. There is no client-side liveness check to gate this on:
// `goal.is_active()` is always true (see above), so just publish the cancel.
std::this_thread::sleep_for(std::chrono::seconds(5));
goal.cancel();

#include <horus/action.hpp>
#include <atomic>
#include <chrono>
#include <cstdio>
#include <thread>

std::atomic<bool> running{true};  // cleared from your shutdown path

horus::ActionServer server("navigate_to_pose");

// Accept handler: receives goal data, returns 0 to accept, 1 to reject
server.set_accept_handler([](const uint8_t* goal_data, size_t len) -> uint8_t {
    // Parse goal JSON, validate parameters
    // Return 0 = accept, 1 = reject
    return 0;  // accept all goals
});

// Execute handler: runs on a dedicated thread per accepted goal. Wrap the raw
// handle, poll for cancellation, publish feedback, and finish the goal.
server.set_execute_handler([](HorusActionGoalHandle* h,
                              const uint8_t* goal_data, size_t len) {
    horus::ActionGoalHandle g(h);
    bool done = false;
    while (!g.is_cancel_requested()) {
        g.publish_feedback(R"({"progress": 0.5})");
        // ... do a slice of work; set done = true when finished ...
        if (done) {
            g.succeed(R"({"arrived": true})");
            return;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    g.canceled(R"({"arrived": false})");
});

// Verify both handlers are set.
if (server.is_ready()) {
    printf("Action server ready\n");
}

// Drive the server on the thread that created it.
while (running) {
    server.process();
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

using AcceptHandler  = uint8_t(*)(const uint8_t* goal, size_t len);
using ExecuteHandler = void(*)(HorusActionGoalHandle* handle, const uint8_t* goal, size_t len);

horus::ServiceClient a("my_svc");
// horus::ServiceClient b = a;           // COMPILE ERROR
horus::ServiceClient b = std::move(a);   // OK

horus::GoalHandle g = client.send_goal("{}");
// horus::GoalHandle g2 = g;             // COMPILE ERROR
horus::GoalHandle g2 = std::move(g);     // OK

#include <horus/service.hpp>
#include <nlohmann/json.hpp>

using json = nlohmann::json;

horus::ServiceServer server("compute_ik");

server.set_handler([](const uint8_t* req, size_t req_len,
                      uint8_t* res, size_t* res_len) -> bool {
    auto request = json::parse(req, req + req_len);

    double x = request["target_x"];
    double y = request["target_y"];
    double z = request["target_z"];

    // Compute inverse kinematics...
    json response = {
        {"joint_angles", {0.1, 0.5, -0.3, 0.0, 1.2, 0.0}},
        {"success", true}
    };

    std::string resp_str = response.dump();
    if (resp_str.size() > *res_len) return false;  // *res_len = buffer capacity
    std::memcpy(res, resp_str.data(), resp_str.size());
    *res_len = resp_str.size();
    return true;
});

#include <atomic>
#include <thread>

// Process A: server
std::atomic<bool> running{true};  // cleared from your shutdown path

horus::ServiceServer server("robot.status");
server.set_handler([](const uint8_t*, size_t,
                      uint8_t* res, size_t* res_len) -> bool {
    const char* s = R"({"battery": 85, "state": "idle"})";
    size_t n = strlen(s);
    if (n > *res_len) return false;   // *res_len arrives holding the capacity
    std::memcpy(res, s, n);
    *res_len = n;
    return true;
});
while (running) {          // drive the server to answer requests
    server.process();
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

// Process B: client
horus::ServiceClient client("robot.status");
auto resp = client.call("{}", std::chrono::milliseconds(500));
if (resp) printf("Robot status: %s\n", resp->c_str());

#include <atomic>
#include <thread>

// Accept/execute handlers are function pointers, so shared state is file-scope.
static std::atomic<bool> nav_active{false};
static std::atomic<bool> nav_done{false};

horus::ActionServer nav_server("navigate");

nav_server.set_accept_handler([](const uint8_t*, size_t) -> uint8_t {
    return nav_active.load() ? 1 : 0;  // reject if already navigating
});

// The goal runs on its own thread. Keep sensor/motor topic I/O OFF this thread
// (topics are single-producer): hand control to the scheduled node via a flag
// and just wait here for completion or cancellation.
nav_server.set_execute_handler([](HorusActionGoalHandle* h, const uint8_t* /*data*/, size_t) {
    horus::ActionGoalHandle g(h);
    nav_done = false;
    nav_active = true;
    while (!nav_done.load()) {
        if (g.is_cancel_requested()) {
            nav_active = false;
            g.canceled(R"({"arrived": false})");
            return;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    g.succeed(R"({"arrived": true})");
});

auto odom_sub = sched.subscribe<horus::msg::Odometry>("odom");
auto cmd_pub  = sched.advertise<horus::msg::CmdVel>("motor.cmd");

// This node pumps the action server AND runs the sensor->motor control loop, so
// all topic I/O stays on one thread.
sched.add("nav_executor").rate(50_hz)
    .tick([&] {
        nav_server.process();  // deliver goals/cancels, publish feedback/results
        if (!nav_active.load()) return;
        auto odom = odom_sub.recv();
        if (!odom) return;
        double dist = std::sqrt(/* dx^2 + dy^2 */);
        if (dist < 0.1) {
            cmd_pub.send(horus::msg::CmdVel{0, 0.0f, 0.0f});
            nav_done = true;  // signals the goal thread to succeed
        } else {
            cmd_pub.send(horus::msg::CmdVel{0, 0.3f, 0.0f});
        }
    }).build();