cyphal_hardware_interface/hardware/simplesync_system.cpp

#include "cyphal_hardware_interface/simplesync_system.hpp"

#include <chrono>
#include <cmath>
#include <cstddef>
#include <hardware_interface/hardware_info.hpp>
#include <limits>
#include <memory>
#include <set>
#include <sstream>
#include <string>
#include <vector>

#include "serial.hpp"

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

#include "simplesync.hpp"

namespace cyphal_hardware_interface {

cyphalSystemHardware::~cyphalSystemHardware() {
  if (serial_port_handle) {
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Closing port '%s'", serial_port_.c_str());
    close(serial_port_handle);
  }
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
    const hardware_interface::HardwareInfo &info) {
  if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  serial_port_ = default_serial_port_;

  if (info_.hardware_parameters.count("port") > 0) {
    serial_port_ = info_.hardware_parameters.at("port");
  } else {
    RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"),
                "No port specified in urdf, using default value");
  }
  if (info_.hardware_parameters.count("try_next_port_number") > 0) {
    std::string try_next_str =
        info_.hardware_parameters.at("try_next_port_number");
    try_next_port_number =
        std::set<std::string>({"True", "true", "1", "Yes", "yes", "Y", "y"})
            .count(try_next_str) > 0;
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Trying next ports if the one provided is not openable");
  }
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Port set as %s",
              serial_port_.c_str());

  std::vector<hardware_interface::ComponentInfo> components;
  components.insert(components.begin(), info_.joints.begin(),
                    info_.joints.end());
  components.insert(components.begin(), info_.sensors.begin(),
                    info_.sensors.end());
  components.insert(components.begin(), info_.gpios.begin(), info_.gpios.end());
  for (const hardware_interface::ComponentInfo &joint : components) {
    // StaubiSystem has exactly two states and one command interface on each
    // joint
    /*if (joint.command_interfaces.size() != 1) {
      RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                   "Joint '%s' has %zu command interfaces found. 1 expected.",
                   joint.name.c_str(), joint.command_interfaces.size());
      return hardware_interface::CallbackReturn::ERROR;
    }*/
    for (const auto &i : joint.command_interfaces) {
      std::string param_name = "command:" + i.name;
      if (joint.parameters.count(param_name) <= 0) {
        RCLCPP_FATAL(
            rclcpp::get_logger("cyphalSystemHardware"),
            "command interface '%s' of joint/gpio '%s' has no parameter "
            "'command:%s'.",
            i.name.c_str(), joint.name.c_str(), i.name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      auto interface_name = joint.parameters.at(param_name);
      if (interfaces_map.count(interface_name) > 0) {
        RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                     "Sync Interface '%s' used twice. FATAL ERROR.",
                     interface_name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      interfaces_map[interface_name] = {
          std::numeric_limits<double>::quiet_NaN(),
          (joint.parameters.count("factor") > 0)
              ? std::stod(joint.parameters.at("factor"))
              : 1.0,
          joint.name, true, i.name};
    }

    for (const auto &i : joint.state_interfaces) {
      std::string param_name = "state:" + i.name;
      if (joint.parameters.count(param_name) <= 0) {
        RCLCPP_FATAL(
            rclcpp::get_logger("cyphalSystemHardware"),
            "state interface '%s' of joint/sensor/gpio '%s' has no parameter "
            "'state:%s'.",
            i.name.c_str(), joint.name.c_str(), i.name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      auto interface_name = joint.parameters.at(param_name);
      if (interfaces_map.count(interface_name) > 0) {
        RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                     "Sync Interface '%s' used twice. FATAL ERROR.",
                     interface_name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      interfaces_map[interface_name] = {
          std::numeric_limits<double>::quiet_NaN(),
          (joint.parameters.count("factor") > 0)
              ? std::stod(joint.parameters.at("factor"))
              : 1.0,
          joint.name, false, i.name};
    }
  }
  // for (const auto &[key, value] : interfaces_map) {
  // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //             "Interface %s added (factor: %f).", key.c_str(), value.factor);
  // }

  return hardware_interface::CallbackReturn::SUCCESS;
} // namespace cyphal_hardware_interface

std::vector<hardware_interface::StateInterface>
cyphalSystemHardware::export_state_interfaces() {
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (auto &[key, value] : interfaces_map) {
    if (!value.command)
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          value.joint, value.control_interface, &value.value));
  }
  /*  for (auto i = 0u; i < info_.joints.size(); i++) {
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          info_.joints[i].name, hardware_interface::HW_IF_POSITION,
          &hw_positions_[i]));
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
          &hw_velocities_[i]));
    }
  */
  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
cyphalSystemHardware::export_command_interfaces() {
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (auto &[key, value] : interfaces_map) {
    if (value.command)
      command_interfaces.emplace_back(hardware_interface::CommandInterface(
          value.joint, value.control_interface, &value.value));
  }
  /* for (auto i = 0u; i < info_.joints.size(); i++) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
         info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
         &hw_commands_[i]));
   }
 */
  return command_interfaces;
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  // const hardware_interface::CallbackReturn result =
  //     can_interface_->connect(can_socket_)
  //         ? hardware_interface::CallbackReturn::SUCCESS
  //         : hardware_interface::CallbackReturn::FAILURE;
  // Open the serial port. Change device path as needed (currently set to an
  // standard FTDI USB-UART cable type device)

  serial_port_handle = open_serial(serial_port_);
  if (!serial_port_handle) {
    RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                 "Error opening port '%s': %s", serial_port_.c_str(),
                 strerror(errno));
    if (try_next_port_number) {
      for (unsigned int tries = 0; tries < 100; tries += 1) {
        size_t digits_idx = serial_port_.size() - 1;
        while (digits_idx > 0 && std::isdigit(serial_port_[digits_idx]))
          digits_idx -= 1;
        digits_idx += 1;
        std::string number_str = serial_port_.substr(digits_idx);
        unsigned int number = number_str.size() > 0 ? std::stol(number_str) : 0;
        serial_port_ =
            serial_port_.substr(0, digits_idx) + std::to_string(number + 1);
        serial_port_handle = open_serial(serial_port_);
        if (!serial_port_handle) {
          RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                       "Error opening port '%s': %s", serial_port_.c_str(),
                       strerror(errno));
        } else
          break;
      }
    }
    if (!serial_port_handle)
      return hardware_interface::CallbackReturn::FAILURE;
  }
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Successfuly opened port '%s'", serial_port_.c_str());

  sync = std::make_unique<cyphalChrono>(
      [this](uint8_t *buf, unsigned int buf_size) {
        std::stringstream ss;
        ss << std::hex;
        for (unsigned int i = 0; i < buf_size; i += 1)
          ss << (int)buf[i] << " ";

        // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                    // "Write to serial: %s", ss.str().c_str());
        write_serial(this->serial_port_handle, buf, buf_size);
        return 0;
      },
      []() { return (std::chrono::steady_clock::now()); });

  sync->request_all_interfaces();

  using namespace std::chrono_literals;
  std::this_thread::sleep_for(100ms); // FIXME: callback
  uint8_t read_buf[1024];
  int read_buf_used =
      read_serial(serial_port_handle, read_buf, sizeof(read_buf));

  sync->handle_stream((uint8_t *)read_buf, read_buf_used);

  for (auto &i : sync->interfaces) {
    if (!i)
      continue;
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Sync-interface %s recived.", i->key.c_str());
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_activate(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // your production code
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Activating ...please wait...");

  /*  for (auto i = 0; i < hw_start_sec_; i++) {
      rclcpp::sleep_for(std::chrono::seconds(1));
      RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                  "%.1f seconds left...", hw_start_sec_ - i);
    }
    // END: This part here is for exemplary purposes - Please do not copy to
    your
    // production code

    // set some default values
    for (auto i = 0u; i < hw_positions_.size(); i++) {
      if (std::isnan(hw_positions_[i])) {
        hw_positions_[i] = 0;
        hw_velocities_[i] = 0;
        hw_commands_[i] = 0;
      }
    }
  */
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Successfully activated!");

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // your production code
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Deactivating ...please wait...");

  /*  for (auto i = 0; i < hw_stop_sec_; i++) {
      rclcpp::sleep_for(std::chrono::seconds(1));
      RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                  "%.1f seconds left...", hw_stop_sec_ - i);
    }
    // END: This part here is for exemplary purposes - Please do not copy to
    your
    // production code

    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Successfully deactivated!");
  */
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type
cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
                               const rclcpp::Duration & /*period*/) {
  // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // your production code
  /* for (std::size_t i = 0; i < hw_velocities_.size(); i++) {
     // Simulate Staubi wheels's movement as a first-order system
     // Update the joint status: this is a revolute joint without any limit.
     // Simply integrates
     hw_positions_[i] = hw_positions_[i] + period.seconds() *
   hw_velocities_[i];

     RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                 "Got position state %.5f and velocity state %.5f for '%s'!",
                 hw_positions_[i], hw_velocities_[i],
                 info_.joints[i].name.c_str());
   }
   // END: This part here is for exemplary purposes - Please do not copy to
   your
   // production code
     */

  // Allocate memory for read buffer, set size according to your needs

  // Read bytes. The behaviour of read() (e.g. does it block?,
  // how long does it block for?) depends on the configuration
  // settings above, specifically VMIN and VTIME
  // char read_buf[256];
  // int num_bytes = read(serial_port, &read_buf, sizeof(read_buf));

  // // n is the number of bytes read. n may be 0 if no bytes were received,
  // and
  // // can also be -1 to signal an error.
  // if (num_bytes < 0) {
  //   printf("Error reading: %s", strerror(errno));
  //   return 1;
  // }

  // // Here we assume we received ASCII data, but you might be sending raw
  // bytes
  // // (in that case, don't try and print it to the screen like this!)
  // printf("Read %i bytes. Received message: %s", num_bytes, read_buf);

  // close(serial_port);
  if (!sync)
    return hardware_interface::return_type::ERROR;

  uint8_t read_buf[1024];
  int read_buf_used =
      read_serial(serial_port_handle, read_buf, sizeof(read_buf));
  sync->handle_stream((uint8_t *)read_buf, read_buf_used);
  for (auto &[key, value] : interfaces_map) {
    // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
    //             "Interface %s added (factor: %f).", key.c_str(),
    //             value.factor);
    if (!value.command) {
      auto n = sync->get_number(key.c_str());
      if (n)
        value.value = value.factor * (*n);
    }
  }

  using namespace std::chrono_literals;
  if (std::chrono::steady_clock::now() - sync->last_pkg_recived >
      std::chrono::milliseconds(1000)) {
    RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                 "last package recivew more than 1000ms ago. aborting!");
    return hardware_interface::return_type::ERROR;
  }

  static int counter = 0;
  if (counter++ % 20 == 0)
    for (auto &i : sync->interfaces) {
      if (!i)
        continue;
      auto number_ptr = sync->get_number(i->key.c_str());
      auto str_ptr = sync->get_string(i->key.c_str());
      if (number_ptr)
        RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                    "Sync: %s = %d", i->key.c_str(), *number_ptr);
      if (str_ptr)
        RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                    "Sync: %s = %s", i->key.c_str(), str_ptr->c_str());
    }

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type
cyphal_hardware_interface ::cyphalSystemHardware::write(
    const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
  // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // your production code

  if (sync) {
    for (auto &[key, value] : interfaces_map) {
      // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
      //             "Interface %s added (factor: %f).", key.c_str(),
      //             value.factor);
      if (value.command) {
        auto ni = sync->get_or_create_interface(0x01, key.c_str());
        if (ni) {

          *(int *)ni->data = (value.value / value.factor);
          ni->send_requested = true;

          // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                      // "update: %s set to %d ", key.c_str(), *(int
                      // *)ni->data);
        }
      }
    }
    sync->update();
  }

  /*  for (auto i = 0u; i < hw_commands_.size(); i++) {
      // Simulate sending commands to the hardware
      RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                  "Got command %.5f for '%s'!", hw_commands_[i],
                  info_.joints[i].name.c_str());

      hw_velocities_[i] = hw_commands_[i];
    }
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Joints successfully written!");
    // END: This part here is for exemplary purposes - Please do not copy to
    your
    // production code
  */
  return hardware_interface::return_type::OK;
}

} // namespace cyphal_hardware_interface

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware,
                       hardware_interface::SystemInterface)