Command Line Interface (C++)

Command line interface using the CreOS SDK client library.

Introduction

The Command Line Interface (CLI) is a tool that uses the CreOS SDK client library to enable interactions with an Avular robot directly from a terminal. You can install the CLI by installing the creos-cli package and its dependencies as follows, replacing <version> with the version number of the CreOS SDK you want to install, and <architecture> with the architecture of your system (e.g., amd64, arm64):

sudo apt install ./creos-cli_<version>_<architecture>.deb ./creos-client_<version>_<architecture>.deb ./creos-utils_<version>_<architecture>.deb

Once installed, the CLI can be invoked using the creos-cli command. The CLI provides a set of commands for subscribing or pulling to/from Data Sources, calling Callables, and publishing to Data Sinks. It also provides a command for listing all available data sources, callables, and data sinks.

A complete list of commands can be found by running:

creos-cli --help

Source code

main.cpp

// Copyright (C) 2024 Avular Holding B.V. - All Rights Reserved
// You may use this code under the terms of the Avular
// Software End-User License Agreement.
//
// You should have received a copy of the Avular
// Software End-User License Agreement license with
// this file, or download it from: avular.com/eula
//
#include <avular_version.h>
#include <fmt/chrono.h>
#include <spdlog/sinks/stdout_color_sinks.h>
#include <spdlog/spdlog.h>
#include <creos/client.hpp>
#include <cxxopts.hpp>
#include <exception>
#include <functional>
#include <nlohmann/json.hpp>
#include <ranges>
#include <string>
#include <thread>
#include <unordered_set>
#include <vector>
 
static void receiveData(const std::string src, const auto& data, bool ndjson_format = true) {
    nlohmann::json data_json = data;
    nlohmann::json json{{"source", src}, {"data", data_json}};
    if (ndjson_format) {
        std::cout << json.dump() << std::endl;
    } else {
        std::cout << json.dump(4) << std::endl;
    }
}
 
void callEnterLowPowerMode(creos::IPowerManagementInterface* interface, std::vector<std::string> request_data) {
    if (request_data.size() == 0) {
        spdlog::debug("Entering low power mode");
        interface->enterLowPowerMode();
    } else if (request_data.size() == 1) {
        time_t wake_time = 0;
        try {
            // Try to parse as the ISO8601 time
            struct tm tm{};
 
            if (strptime(request_data[0].c_str(), "%Y-%m-%dT%H:%M:%S", &tm) != nullptr) {
                wake_time = mktime(&tm);
            } else if (strptime(request_data[0].c_str(), "%Y-%m-%dT%H:%M:%S%z", &tm) != nullptr) {
                wake_time = mktime(&tm);
            } else {
                // Try to parse as a unix timestamp
                wake_time = std::stoll(request_data[0]);
            }
        } catch (std::exception& e) {
            spdlog::error("Failed to parse wake_time, It must be a UNIX timestamp or an ISO8601 date time string");
            return;
        }
        spdlog::debug("Entering low power mode with wake_time: {}", wake_time);
        interface->enterLowPowerMode(creos::RobotClock::from_time_t(wake_time));
    } else {
        spdlog::error("Invalid number of arguments, expected 0 or 1");
        return;
    }
    std::cout << "{}" << std::endl;
}
 
void getSource(auto interface, auto method, bool ndjson_format = true, int timeout_ms = 1500) {
    auto res = (interface->*method)(timeout_ms);
    nlohmann::json json = res;
    if (ndjson_format) {
        std::cout << json.dump() << std::endl;
    } else {
        std::cout << json.dump(4) << std::endl;
    }
}
 
template <typename MessageType, typename PublishFunction>
void publishMessage(const std::string& sink, const std::optional<std::string>& data,
                    const std::string& message_type_name, PublishFunction publishFunc,
                    const std::optional<MessageType>& default_message = std::nullopt) {
    MessageType message;
    try {
        if (data.has_value()) {
            nlohmann::json data_json = nlohmann::json::parse(data.value());
            nlohmann::from_json(data_json, message);
        } else {
            spdlog::info("No data provided, using default message data");
            if (default_message.has_value()) {
                message = default_message.value();
            }
        }
    } catch (const std::exception& e) {
        spdlog::error("Invalid JSON format, run using: --publish {} --publish {} --publish'{}'", sink,
                      message_type_name, nlohmann::json(message).dump());
        exit(1);
    }
 
    try {
        spdlog::info("Publishing {} message to: {}", message_type_name, sink);
        publishFunc(message);
    } catch (const std::exception& e) {
        spdlog::error("Failed to publish message: {}", e.what());
        exit(1);
    }
}
 
// The data is passed as a JSON string which contains ',' separated values.
// Which are split into individual arguments by the cxxopts library.
// Therefore we need to join the remaining vector to get the full JSON string
std::string argumentsToJsonStr(const std::vector<std::string>& args) {
    if (args.size() == 0) {
        return "{}";
    }
 
    std::string json_str = "";
    for (size_t i = 0; i < args.size(); ++i) {
        if (i > 0) {
            json_str += ',';
        }
        json_str += args[i];
    }
    return json_str;
}
 
void handleList(creos::ISensorsInterface* sensors, creos::ITransformsInterface* transforms,
                creos::ISystemInfoInterface* system_info, creos::ISetpointControlInterface* setpoint_control,
                creos::IPowerManagementInterface* power_management, creos::IDiagnosticsInterface* diagnostics);
void handleSubscribe(cxxopts::ParseResult& arguments, creos::ISensorsInterface* sensors,
                     creos::ITransformsInterface* transforms, creos::ISystemInfoInterface* system_info,
                     creos::ISetpointControlInterface* setpoint_control, creos::IDiagnosticsInterface* diagnostics,
                     bool ndjson_format, bool statistics);
void handleCall(cxxopts::ParseResult& arguments, creos::IPowerManagementInterface* power_management,
                creos::ISetpointControlInterface* setpoint_control);
void handleGet(cxxopts::ParseResult& arguments, creos::ISystemInfoInterface* system_info,
               creos::ISetpointControlInterface* setpoint_control, creos::IDiagnosticsInterface* diagnostics,
               bool ndjson_format);
void handlePublish(cxxopts::ParseResult& arguments, creos::ISetpointControlInterface* setpoint_control);
 
int main(int argc, char** argv) {
    // Setup the spdlog output to output to stderr, this will allow other tools to parse the stdout which will only contain data.
    auto console_sink = std::make_shared<spdlog::sinks::stderr_color_sink_st>();
    spdlog::set_default_logger(std::make_shared<spdlog::logger>("console", console_sink));
 
    cxxopts::Options options(argv[0], "CLI tool to interact with avular robots");
    // clang-format off
    options.add_options()
        ("h,host", "Host address of the robot", cxxopts::value<std::string>())
        ("version", "Print version")
        ("v,verbose", "Enable verbose output")
        ("help", "Print help")
        ("echo", "Echo data received from the selected sources (to echo multiple sources at the same time, add "
                 "multiple --echo arguments: --echo source_1 --echo source_2)",
            cxxopts::value<std::vector<std::string>>())
        ("statistics", "Print statistics of the received data (can be used with multiple sources at the same time: "
                       "--statistics source_1 --statistics source_2)",
            cxxopts::value<std::vector<std::string>>())
        ("window", "Window size for statistics in messages", cxxopts::value<unsigned>()->default_value("1000"))
        ("list", "List the available sources, sinks and callables")
        ("call", "Call a callable", cxxopts::value<std::vector<std::string>>())
        ("get", "Get data from a source", cxxopts::value<std::string>())
        ("publish", "Publish data to a sink", cxxopts::value<std::vector<std::string>>())
        ("ndjson", "Output JSON in NDJSON format (one line per JSON object) instead of pretty printed format", cxxopts::value<bool>()->default_value("false"))
        ;
    // clang-format on
 
    cxxopts::ParseResult arguments;
    try {
        arguments = options.parse(argc, argv);
    } catch (cxxopts::OptionException& e) {
        spdlog::critical("Argument error: {}", e.what());
        return 1;
    }
 
    if (arguments.count("help") || argc == 1) {
        std::cout << options.help() << std::endl;
        return 0;
    }
 
    if (arguments.count("version")) {
        PrintAvularVersion();
        return 0;
    }
 
    if (arguments.count("verbose")) {
        spdlog::set_level(spdlog::level::debug);
    }
 
    bool ndjson_format = arguments["ndjson"].as<bool>();
 
    std::unique_ptr<creos::Client> client;
    if (arguments.count("host")) {
        client = std::make_unique<creos::Client>(arguments["host"].as<std::string>());
    } else {
        client = std::make_unique<creos::Client>();
    }
 
    auto sensors = client->sensors();
    auto power_management = client->power_management();
    auto transforms = client->transforms();
    auto system_info = client->system_info();
    auto setpoint_control = client->setpoint_control();
    auto diagnostics = client->diagnostics();
 
    if (arguments.count("list")) {
        handleList(sensors, transforms, system_info, setpoint_control, power_management, diagnostics);
    }
    if (arguments.count("echo")) {
        handleSubscribe(arguments, sensors, transforms, system_info, setpoint_control, diagnostics, ndjson_format,
                        false);
    }
    if (arguments.count("statistics")) {
        handleSubscribe(arguments, sensors, transforms, system_info, setpoint_control, diagnostics, ndjson_format,
                        true);
    }
    if (arguments.count("call")) {
        handleCall(arguments, power_management, setpoint_control);
    }
    if (arguments.count("get")) {
        handleGet(arguments, system_info, setpoint_control, diagnostics, ndjson_format);
    }
    if (arguments.count("publish")) {
        handlePublish(arguments, setpoint_control);
    }
}
 
void handleList(creos::ISensorsInterface* sensors, creos::ITransformsInterface* transforms,
                creos::ISystemInfoInterface* system_info, creos::ISetpointControlInterface* setpoint_control,
                creos::IPowerManagementInterface* power_management, creos::IDiagnosticsInterface* diagnostics) {
    // List the available sources and callables
    std::cout << "Available data sources:" << std::endl;
    if (sensors) {
        std::cout << " - sensors.lidar_points.front" << std::endl;
        std::cout << " - sensors.odometry" << std::endl;
        std::cout << " - sensors.pose" << std::endl;
        std::cout << " - sensors.gnss_fix" << std::endl;
        std::cout << " - sensors.remote_control" << std::endl;
        std::cout << " - sensors.wheel_states" << std::endl;
        std::cout << " - sensors.magnetic_field.body" << std::endl;
        std::cout << " - sensors.barometer" << std::endl;
        std::cout << " - sensors.range.downwards_lidar" << std::endl;
        std::cout << " - sensors.imu.body" << std::endl;
    }
    if (transforms) {
        std::cout << " - transforms.dynamic_transforms" << std::endl;
        std::cout << " - transforms.static_transforms" << std::endl;
    }
    if (system_info) {
        std::cout << " - system_info.system_info" << std::endl;
        std::cout << " - system_info.battery_status" << std::endl;
    }
    if (setpoint_control) {
        std::cout << " - setpoint_control.current_control_source" << std::endl;
    }
    if (diagnostics) {
        std::cout << " - diagnostics.user_events" << std::endl;
        std::cout << " - diagnostics.state" << std::endl;
    }
 
    std::cout << "Available data sinks:" << std::endl;
    if (setpoint_control) {
        std::cout << " - setpoint_control.cmd_vel_user" << std::endl;
        std::cout << " - setpoint_control.state_reference" << std::endl;
    }
 
    std::cout << "Available callables:" << std::endl;
    if (power_management) {
        std::cout << " - power_management.enter_low_power_mode[,wake_time]" << std::endl;
        std::cout << " - power_management.exit_low_power_mode" << std::endl;
        std::cout << " - power_management.shutdown" << std::endl;
        std::cout << " - power_management.reboot" << std::endl;
    }
    if (setpoint_control) {
        std::cout << " - setpoint_control.set_control_source" << std::endl;
        std::cout << " - setpoint_control.command" << std::endl;
    }
 
    std::cout << "Available getters:" << std::endl;
    if (system_info) {
        std::cout << " - system_info.system_info" << std::endl;
        std::cout << " - system_info.battery_status" << std::endl;
    }
    if (setpoint_control) {
        std::cout << " - setpoint_control.current_control_source" << std::endl;
    }
    if (diagnostics) {
        std::cout << " - diagnostics.state" << std::endl;
    }
}
 
using float_millis = std::chrono::duration<float, std::milli>;
 
void handleSubscribe(cxxopts::ParseResult& arguments, creos::ISensorsInterface* sensors,
                     creos::ITransformsInterface* transforms, creos::ISystemInfoInterface* system_info,
                     creos::ISetpointControlInterface* setpoint_control, creos::IDiagnosticsInterface* diagnostics,
                     bool ndjson_format, bool statistics) {
    auto sources = std::vector<std::string>();
    if (statistics) {
        sources = arguments["statistics"].as<std::vector<std::string>>();
    } else {
        sources = arguments["echo"].as<std::vector<std::string>>();
    }
    auto source_statistics = std::map<std::string, std::deque<float_millis>>();
    for (const auto& source : sources) {
        source_statistics[source] = std::deque<float_millis>();
    }
    auto statistics_window = arguments["window"].as<unsigned>();
    std::unordered_set<std::string> registered_sources;
 
    auto isSpecified = [&](const std::string& expr, const std::string& interfaceName, const std::string& sourceName) {
        // Accept the wildcard match
        if (expr == "*") {
            return true;
        }
        auto separator_it = expr.find('.');
        if (separator_it == std::string::npos) {
            return false;
        }
        auto interface_part = expr.substr(0, separator_it);
        auto source_part = expr.substr(separator_it + 1);
        auto match_interface = interface_part == "*" || interface_part == interfaceName;
        auto match_source = source_part == "*" || source_part == sourceName;
        return match_interface && match_source;
    };
 
    auto subscribeIfSpecified = [&](const std::string& expr, const std::string& interfaceName,
                                    const std::string& sourceName, auto interface, auto subscribeFunc,
                                    bool& subscription_registered) {
        if (!isSpecified(expr, interfaceName, sourceName)) {
            return;
        }
        if (registered_sources.find(expr) != registered_sources.end()) {
            return;
        }
        try {
            spdlog::info("Subscribing to: {}.{}", interfaceName, sourceName);
 
            std::string key = interfaceName + "." + sourceName;
            registered_sources.insert(key);
 
            // Call the subscribe function of the interface
            (interface->*subscribeFunc)(
                [key, ndjson_format, statistics, &source_statistics, &statistics_window,
                 last_time = std::chrono::system_clock::time_point()](const auto& data) mutable {
                    if (statistics) {
                        if (last_time.time_since_epoch().count() > 0) {
                            source_statistics[key].push_back(
                                std::chrono::duration_cast<float_millis>(std::chrono::system_clock::now() - last_time));
                            if (source_statistics[key].size() > statistics_window) {
                                source_statistics[key].pop_front();
                            }
                        }
                        last_time = std::chrono::system_clock::now();
                    } else {
                        receiveData(key, data, ndjson_format);
                    }
                });
            subscription_registered = true;
        } catch (const creos::Exception& e) {
            spdlog::warn("Failed to subscribe to {}.{}: {}", interfaceName, sourceName, e.what());
        }
    };
 
    auto subscribeNamedIfSpecified = [&](const std::string& expr, const std::string& interfaceName,
                                         const std::string& sourceName, auto sourceId, auto interface,
                                         auto subscribeFunc, bool& subscription_registered) {
        if (!isSpecified(expr, interfaceName, sourceName)) {
            return;
        }
        if (registered_sources.find(expr) != registered_sources.end()) {
            return;
        }
        try {
            spdlog::info("Subscribing to: {}.{}", interfaceName, sourceName);
 
            std::string key = interfaceName + "." + sourceName;
            registered_sources.insert(key);
            (interface->*subscribeFunc)(
                sourceId, [key, ndjson_format, statistics, &source_statistics, &statistics_window,
                           last_time = std::chrono::system_clock::time_point()](const auto& data) mutable {
                    if (statistics) {
                        if (last_time.time_since_epoch().count() > 0) {
                            source_statistics[key].push_back(
                                std::chrono::duration_cast<float_millis>(std::chrono::system_clock::now() - last_time));
                            if (source_statistics[key].size() > statistics_window) {
                                source_statistics[key].pop_front();
                            }
                        }
                        last_time = std::chrono::system_clock::now();
                    } else {
                        receiveData(key, data, ndjson_format);
                    }
                });
            subscription_registered = true;
        } catch (const creos::Exception& e) {
            spdlog::warn("Failed to subscribe to {}.{}: {}", interfaceName, sourceName, e.what());
        }
    };
 
    for (const auto& source : sources) {
        bool subscription_registered = false;
 
        subscribeNamedIfSpecified(source, "sensors", "lidar_points.front", creos_messages::Lidar3dId::kFront, sensors,
                                  &creos::ISensorsInterface::subscribeTo3dLidar, subscription_registered);
        subscribeIfSpecified(source, "sensors", "odometry", sensors, &creos::ISensorsInterface::subscribeToOdometry,
                             subscription_registered);
        subscribeIfSpecified(source, "sensors", "pose", sensors, &creos::ISensorsInterface::subscribeToPose,
                             subscription_registered);
        subscribeIfSpecified(source, "sensors", "gnss_fix", sensors, &creos::ISensorsInterface::subscribeToGnss,
                             subscription_registered);
        subscribeIfSpecified(source, "sensors", "remote_control", sensors,
                             &creos::ISensorsInterface::subscribeToRemoteController, subscription_registered);
        subscribeIfSpecified(source, "sensors", "wheel_states", sensors,
                             &creos::ISensorsInterface::subscribeToWheelState, subscription_registered);
        subscribeNamedIfSpecified(source, "sensors", "magnetic_field.body", creos_messages::MagneticFieldId::kBody,
                                  sensors, &creos::ISensorsInterface::subscribeToMagneticField,
                                  subscription_registered);
        subscribeIfSpecified(source, "sensors", "barometer", sensors, &creos::ISensorsInterface::subscribeToBarometer,
                             subscription_registered);
        subscribeNamedIfSpecified(source, "sensors", "range.downwards_lidar",
                                  creos_messages::RangeSensorId::kDownwardsLidar, sensors,
                                  &creos::ISensorsInterface::subscribeToRangeSensor, subscription_registered);
        subscribeNamedIfSpecified(source, "sensors", "imu.body", creos_messages::ImuId::kBody, sensors,
                                  &creos::ISensorsInterface::subscribeToImu, subscription_registered);
        subscribeIfSpecified(source, "transforms", "dynamic_transforms", transforms,
                             &creos::ITransformsInterface::subscribeToDynamicTransforms, subscription_registered);
        subscribeIfSpecified(source, "transforms", "static_transforms", transforms,
                             &creos::ITransformsInterface::subscribeToStaticTransforms, subscription_registered);
        subscribeIfSpecified(source, "system_info", "system_info", system_info,
                             &creos::ISystemInfoInterface::subscribeToSystemInfo, subscription_registered);
        subscribeIfSpecified(source, "system_info", "battery_status", system_info,
                             &creos::ISystemInfoInterface::subscribeToBatteryStatus, subscription_registered);
        subscribeIfSpecified(source, creos::ISetpointControlInterface::name, "current_control_source", setpoint_control,
                             &creos::ISetpointControlInterface::subscribeToCurrentControlSource,
                             subscription_registered);
        subscribeIfSpecified(source, "diagnostics", "user_events", diagnostics,
                             &creos::IDiagnosticsInterface::subscribeToUserEvents, subscription_registered);
        subscribeIfSpecified(source, "diagnostics", "state", diagnostics,
                             &creos::IDiagnosticsInterface::subscribeToState, subscription_registered);
 
        if (!subscription_registered) {
            spdlog::error("Unknown source: {}", source);
        }
    }
 
    while (true) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        if (statistics) {
            if (!ndjson_format) {
                fmt::print("Statistics at {}:\n", std::chrono::system_clock::now());
            }
            for (const auto& [source, durations] : source_statistics) {
                if (durations.size() < 2) {
                    if (!ndjson_format) {
                        std::cout << "- " << source << ": <not enough data>" << std::endl;
                    }
                    continue;
                }
 
                // Drop first duration, as the first sample always comes in immediately after subscribing due to
                // caching behavior.
                auto durations_view = std::views::drop(durations, 1);
                auto total_duration = std::accumulate(durations_view.begin(), durations_view.end(), float_millis(0));
                auto avg_duration = total_duration / durations_view.size();
                auto frequency = avg_duration.count() > 0
                                     ? std::chrono::duration<double>(1) /
                                           std::chrono::duration_cast<std::chrono::duration<double>>(avg_duration)
                                     : 0;
                auto min_duration = *std::min_element(durations_view.begin(), durations_view.end());
                auto max_duration = *std::max_element(durations_view.begin(), durations_view.end());
                auto variance = std::accumulate(durations_view.begin(), durations_view.end(), float_millis(0),
                                                [avg_duration](auto acc, auto duration) {
                                                    return acc + float_millis((duration - avg_duration).count() *
                                                                              (duration - avg_duration).count());
                                                }) /
                                durations_view.size();
                auto std_dev = float_millis(std::sqrt(variance.count()));
                if (ndjson_format) {
                    std::cout << nlohmann::json::object({
                                     {"timestamp", std::chrono::duration_cast<std::chrono::seconds>(
                                                       std::chrono::system_clock::now().time_since_epoch())
                                                       .count()},
                                     {"source", source},
                                     {"total", durations.size()},
                                     {"freq", frequency},
                                     {"avg", avg_duration.count()},
                                     {"min", min_duration.count()},
                                     {"max", max_duration.count()},
                                     {"sd", std_dev.count()},
                                 })
                              << std::endl;
                } else {
                    std::cout << fmt::format(
                                     "- {}: \n\t\ttotal={}  \n\t\tfreq={:.2f}hz \n\t\tavg={}, min={}, max={}, sd={}",
                                     source, durations.size(), frequency, avg_duration, min_duration, max_duration,
                                     std_dev)
                              << std::endl;
                }
            }
        }
    }
}
 
void handleCall(cxxopts::ParseResult& arguments, creos::IPowerManagementInterface* power_management,
                creos::ISetpointControlInterface* setpoint_control) {
    {
        auto call_arguments = arguments["call"].as<std::vector<std::string>>();
        try {
            auto callable = call_arguments.at(0);
            auto callable_args = std::vector<std::string>(call_arguments.begin() + 1, call_arguments.end());
 
            auto interface_name = callable.substr(0, callable.find('.'));
 
            // Power management interface
            if (interface_name == "power_management") {
                if (!power_management) {
                    spdlog::error("Power management interface not available");
                    exit(1);
                }
                if (callable == "power_management.enter_low_power_mode") {
                    callEnterLowPowerMode(power_management, callable_args);
                } else if (callable == "power_management.exit_low_power_mode") {
                    power_management->exitLowPowerMode();
                    std::cout << "Success" << std::endl;
                } else if (callable == "power_management.shutdown") {
                    power_management->shutdown();
                    std::cout << "Success" << std::endl;
                } else if (callable == "power_management.reboot") {
                    power_management->reboot();
                    std::cout << "Success" << std::endl;
                } else {
                    spdlog::error("unknown callable: {}", callable);
                }
            }
            // Setpoint control interface
            else if (interface_name == "setpoint_control") {
                if (!setpoint_control) {
                    spdlog::error("Setpoint control interface not available");
                    exit(1);
                }
 
                if (callable == "setpoint_control.set_control_source") {
                    if (callable_args.size() != 1) {
                        spdlog::error("Invalid number of arguments, expected 1");
                        exit(1);
                    }
 
                    creos_messages::ControlSource control_source =
                        creos_messages::ControlSource(std::stoi(callable_args.at(0)));
                    setpoint_control->setControlSource(control_source);
                    std::cout << "Success" << std::endl;
                } else if (callable == "setpoint_control.command") {
                    if (callable_args.size() != 1) {
                        spdlog::error("Invalid number of arguments, expected 1");
                        exit(1);
                    }
 
                    creos_messages::Command command;
                    command.action = creos_messages::Command::Action(std::stoi(callable_args.at(0)));
                    setpoint_control->sendCommand(command);
                    std::cout << "Success" << std::endl;
                } else {
                    spdlog::error("unknown callable: {}", callable);
                }
            } else {
                spdlog::error("unknown interface: {}", interface_name);
                exit(1);
            }
        } catch (const creos::ServiceCallError& e) {
            std::cout << "Call failed[" << static_cast<int>(e.code()) << "]: " << e.what() << std::endl;
        } catch (const creos::TimeoutError& e) {
            std::cout << "Call timed out: " << e.what() << std::endl;
        } catch (const creos::UnsupportedError& e) {
            std::cout << "Call unsupported: " << e.what() << std::endl;
        } catch (const std::exception& e) {
            std::cout << "Call failed: " << e.what() << std::endl;
            exit(1);
        }
    }
}
 
void handleGet(cxxopts::ParseResult& arguments, creos::ISystemInfoInterface* system_info,
               creos::ISetpointControlInterface* setpoint_control, creos::IDiagnosticsInterface* diagnostics,
               bool ndjson_format) {
    auto source = arguments["get"].as<std::string>();
 
    if (source == "system_info.system_info") {
        getSource(system_info, &creos::ISystemInfoInterface::getSystemInfo, ndjson_format);
    } else if (source == "system_info.battery_status") {
        getSource(system_info, &creos::ISystemInfoInterface::getBatteryStatus, ndjson_format);
    } else if (source == "setpoint_control.current_control_source") {
        getSource(setpoint_control, &creos::ISetpointControlInterface::getCurrentControlSource, ndjson_format);
    } else if (source == "diagnostics.state") {
        getSource(diagnostics, &creos::IDiagnosticsInterface::getState, ndjson_format);
    } else {
        spdlog::error("unknown source: {}", source);
        exit(1);
    }
}
 
void handlePublish(cxxopts::ParseResult& arguments, creos::ISetpointControlInterface* setpoint_control) {
    auto pub_arguments = arguments["publish"].as<std::vector<std::string>>();
    try {
        if (pub_arguments.size() < 2) {
            spdlog::error(
                "Invalid number of arguments, usage: --publish sink,message_type or --publish "
                "sink,message_type,'json_data'");
            exit(1);
        }
 
        auto pub_sink = pub_arguments.at(0);
        auto pub_message_type = pub_arguments.at(1);
 
        std::optional<std::string> pub_data = std::nullopt;
        if (pub_arguments.size() >= 3) {
            pub_data = argumentsToJsonStr(std::vector<std::string>(pub_arguments.begin() + 2, pub_arguments.end()));
        }
 
        auto interface_name = pub_sink.substr(0, pub_sink.find('.'));
 
        // Setpoint control interface
        if (interface_name == "setpoint_control") {
 
            if (!setpoint_control) {
                spdlog::error("Setpoint control interface not available");
                exit(1);
            }
 
            if (pub_sink == "setpoint_control.cmd_vel_user") {
                if (pub_message_type == "Twist") {
                    publishMessage<creos_messages::Twist>(
                        pub_sink, pub_data, pub_message_type,
                        [&](const auto& message) { setpoint_control->publishVelocityCommand(message); });
                } else {
                    spdlog::error("Invalid message type: {}", pub_message_type);
                    exit(1);
                }
            } else if (pub_sink == "setpoint_control.state_reference") {
                if (pub_message_type == "StateReference") {
                    creos_messages::StateReference default_message;
                    default_message.orientation_mode =
                        creos_messages::StateReference::OrientationMode::kAngularVelocity;
                    default_message.translation_mode = creos_messages::StateReference::TranslationMode::kPosition;
                    publishMessage<creos_messages::StateReference>(
                        pub_sink, pub_data, pub_message_type,
                        [&](const auto& message) { setpoint_control->publishStateReference(message); },
                        default_message);
                } else {
                    spdlog::error("Invalid message type: {}", pub_message_type);
                    exit(1);
                }
            } else {
                spdlog::error("unknown sink: {}", pub_sink);
                exit(1);
            }
        } else {
            spdlog::error("unknown interface: {}", interface_name);
            exit(1);
        }
    } catch (const std::exception& e) {
        std::cerr << e.what() << '\n';
    }
}