361 lines
14 KiB
C++
361 lines
14 KiB
C++
#include "cyphal_hardware_interface/cyphal_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 "cyphal.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());
|
|
|
|
|
|
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*/) {
|
|
|
|
// 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)
|