motorcortex-rust 0.5.0

/*
 * This is a template for the main.cpp file, where MOTORCORTEX Tasks are managed
 */

#include "MainControlLoop.h"

#include <chrono>
#include <csignal>
#include <cstdlib>
#include <thread>

using namespace mcx;

void run(cmd_line::Config config) {

  nng_log_set_logger(nng_stderr_logger);
  nng_log_set_level(nng_log_level::NNG_LOG_DEBUG);

  // create the parameter server
  parameter_server::Parameter paramServer;
  // create the root of the parameter tree
  paramServer.create("root", nullptr);

  // create the logger task from the built-in log module
  mcx::log::Module logger(config.path("Log"));
  logger.create("logger", &paramServer);
  // create and configure log output task
  mcx::container::Task loggerTask("Logger_task", &paramServer);
  loggerTask.add(&logger);
  loggerTask.configure();

  // print system configuration
  log_info("System configuration:\n{}", config.toString());

  // load and create user parameters
  user_parameters::Module userParameters(config.path("UserParameters"));
  userParameters.create("UserParameters", &paramServer);

  // create the watchdog module
  watchdog::Module watchdog(config.mode(), watchdog::WatchdogType::EXTERNAL, "/dev/null");
  watchdog.create("Watchdog", &paramServer);

  // create the main control loop module
  MainControlLoop mainControlLoop;
  mainControlLoop.create("Control", &paramServer);
  // create and configure the control task
  container::Task controlsTask("Control_task", &paramServer);
  // add the main control loop, the watchdog and user parameters modules to the control task
  controlsTask.add({&mainControlLoop, &watchdog, &userParameters});
  controlsTask.configure();

  // create the field bus manager
  fbus::Manager fbusManager;
  // create the field bus master configuration
  auto fbusModule = fbusManager.create(config.mode(), &paramServer, config.path("Fieldbus"));
  // create and configure field bus task
  container::Task fbusTask("Fieldbus_task", &paramServer);
  fbusTask.add(fbusModule);
  fbusTask.configure();

  // create the req/rep server
  comm::RequestReply reqrep;

  // create the publisher module
  auto connectionData = config.server("Default");
  connectionData.number_of_threads = 8;
  comm::Publisher publisher(reqrep, connectionData);
  publisher.create("ParamPub", &paramServer);
  // create and configure the communication task
  container::Task commTask("Comm_task", &paramServer);
  commTask.add(&publisher);
  commTask.configure();

  // req/rep is configured after all the modules, it caches the parameter tree
  reqrep.configure(&paramServer, cmd_line::path(config.path("Control")));

  // load parameter values from the file
  parameter_server::load(config.path("Control"), &paramServer);
  // loading links and setting startup input values
  parameter_server::link(config.path("Link"), &paramServer, config.mode());

  // start all the tasks
  loggerTask.start(config.task("Logger_task"));
  commTask.start(config.task("Comm_task"));
  controlsTask.start(config.task("Control_task"));
  fbusTask.start(config.task("Fieldbus_task"));
  // start the req/rep server
  bool isConnected = reqrep.start(connectionData);
  log_assert(isConnected, "Failed to start Req/Rep server");

  // Optional self-termination watchdog: set MCX_TEST_SERVER_LIFETIME_SEC to
  // have the server raise SIGTERM after that many seconds. Used by reconnect
  // integration tests to simulate a server crash without test-harness help.
  const char* lifetime_env = std::getenv("MCX_TEST_SERVER_LIFETIME_SEC");
  if (lifetime_env != nullptr) {
    int secs = std::atoi(lifetime_env);
    if (secs > 0) {
      std::thread([secs]() {
        std::this_thread::sleep_for(std::chrono::seconds(secs));
        std::raise(SIGTERM);
      }).detach();
    }
  }

  // req/rep server is used to create the main loop because it is blocking
  while (utils::running()) {
    reqrep.iterate();
  }

  // stop req/reply server
  reqrep.stop();
  // stops all the tasks
  commTask.stop();
  controlsTask.stop();
  fbusTask.stop();
  loggerTask.stop();
  // clear all allocated resources
  paramServer.destroy();
}

int main(int argc, char** argv) {


  // parse command line, register components
  auto config = mcx::cmd_line::parse(argc, argv,
                                     {
                                         //  {"Motorcortex-control", control::version()},
                                         //  {"Motorcortex-math",    math::version()}
                                     });

  utils::configureLogger(config.logLevel());
  // start low latency and CPU isolation
  utils::startRealTime(config.realtime());
  // setup and run user controls
  run(config);
  // stop low latency, removes CPU isolation
  utils::stopRealTime(config.realtime());

  return 0;
}