From 355cedfd876bcb8c3971878cd9115de2cf8f820f Mon Sep 17 00:00:00 2001 From: James N <59348282+RandomSpaceship@users.noreply.github.com> Date: Sat, 21 Dec 2024 00:32:11 +1000 Subject: [PATCH] perseus_hardware: Initial MCB+VESC implementation done --- .../src/perseus_hardware/CMakeLists.txt | 3 +- .../perseus_hardware/include/mcb_system.hpp | 20 ++ .../perseus_hardware/include/vesc_system.hpp | 22 +- .../priv_include/system_common.hpp | 14 +- .../src/perseus_hardware/src/mcb_system.cpp | 298 +++++++++++++++--- .../src/perseus_hardware/src/vesc_system.cpp | 255 ++++++++++++--- 6 files changed, 515 insertions(+), 97 deletions(-) diff --git a/software/ros_ws/src/perseus_hardware/CMakeLists.txt b/software/ros_ws/src/perseus_hardware/CMakeLists.txt index 52311858..e6ab5a29 100644 --- a/software/ros_ws/src/perseus_hardware/CMakeLists.txt +++ b/software/ros_ws/src/perseus_hardware/CMakeLists.txt @@ -80,8 +80,7 @@ target_include_directories( ${PROJECT_NAME} PRIVATE $) -target_link_libraries(${PROJECT_NAME} PUBLIC hi_can) -target_link_libraries(${PROJECT_NAME} PRIVATE hi_can_raw) +target_link_libraries(${PROJECT_NAME} PUBLIC hi_can_raw) ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/software/ros_ws/src/perseus_hardware/include/mcb_system.hpp b/software/ros_ws/src/perseus_hardware/include/mcb_system.hpp index f13ebf74..01fb4512 100644 --- a/software/ros_ws/src/perseus_hardware/include/mcb_system.hpp +++ b/software/ros_ws/src/perseus_hardware/include/mcb_system.hpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include #include @@ -19,15 +21,33 @@ namespace perseus_hardware { public: RCLCPP_SHARED_PTR_DEFINITIONS(McbSystemHardware) + virtual ~McbSystemHardware(); hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: + // note: using std::optional so we can delay initialisation until on_configure + std::optional _canInterface; + std::optional _packetManager; + + /// @brief The last time we tried to transmit and had an error + /// @details Used to throttle transmissions (and error messages) so we don't build up a massive transmit queue when the bus comes online again + rclcpp::Time _lastTransmitError; + + std::vector _parameterGroups; + std::vector _escIds; + std::vector _commandSpeeds; + std::vector _realSpeeds; + std::vector _realPositions; + std::vector _realCurrents; + std::vector _realTemperatures; }; } \ No newline at end of file diff --git a/software/ros_ws/src/perseus_hardware/include/vesc_system.hpp b/software/ros_ws/src/perseus_hardware/include/vesc_system.hpp index a51080b3..cb13b62e 100644 --- a/software/ros_ws/src/perseus_hardware/include/vesc_system.hpp +++ b/software/ros_ws/src/perseus_hardware/include/vesc_system.hpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include #include @@ -21,27 +21,33 @@ namespace perseus_hardware { public: RCLCPP_SHARED_PTR_DEFINITIONS(VescSystemHardware) + virtual ~VescSystemHardware(); hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; - auto& get_logger() const { return *_logger; } - auto get_clock() const { return _clock; } - auto& get_info() const { return info_; } - private: + // note: using std::optional so we can delay initialisation until on_configure + std::optional _canInterface; std::optional _packetManager; + /// @brief The last time we tried to transmit and had an error + /// @details Used to throttle transmissions (and error messages) so we don't build up a massive transmit queue when the bus comes online again + rclcpp::Time _lastTransmitError; + + std::vector _parameterGroups; + std::vector _vescIds; std::vector _commandSpeeds; std::vector _realSpeeds; std::vector _realPositions; - - std::shared_ptr _logger; - rclcpp::Clock::SharedPtr _clock; + std::vector _realCurrents; + std::vector _realTemperatures; }; } \ No newline at end of file diff --git a/software/ros_ws/src/perseus_hardware/priv_include/system_common.hpp b/software/ros_ws/src/perseus_hardware/priv_include/system_common.hpp index 4c5f69fe..a5ef0076 100644 --- a/software/ros_ws/src/perseus_hardware/priv_include/system_common.hpp +++ b/software/ros_ws/src/perseus_hardware/priv_include/system_common.hpp @@ -4,7 +4,7 @@ #include #include -#define CHECK_INTERFACE_COUNT(_logger, _joint, _interface_type, _interface_name, _count) \ +#define CHECK_EQ_INTERFACE_COUNT(_logger, _joint, _interface_type, _interface_name, _count) \ do \ { \ if ((_joint)._interface_type.size() != (_count)) \ @@ -16,6 +16,18 @@ return hardware_interface::CallbackReturn::ERROR; \ } \ } while (0) +#define CHECK_GE_INTERFACE_COUNT(_logger, _joint, _interface_type, _interface_name, _count) \ + do \ + { \ + if ((_joint)._interface_type.size() >= (_count)) \ + { \ + RCLCPP_FATAL((_logger), "Joint '%s' has %zu " _interface_name " interfaces found, expected at least %d", \ + (_joint).name.c_str(), \ + (_joint)._interface_type.size(), \ + _count); \ + return hardware_interface::CallbackReturn::ERROR; \ + } \ + } while (0) #define CHECK_INTERFACE_NAME(_logger, _joint, _interface_type, _interface_name, _interface_index, _expected_name) \ do \ diff --git a/software/ros_ws/src/perseus_hardware/src/mcb_system.cpp b/software/ros_ws/src/perseus_hardware/src/mcb_system.cpp index 5908c649..40dc7906 100644 --- a/software/ros_ws/src/perseus_hardware/src/mcb_system.cpp +++ b/software/ros_ws/src/perseus_hardware/src/mcb_system.cpp @@ -1,70 +1,292 @@ #include "mcb_system.hpp" +#include +#include +#include +#include +#include +#include #include +#include "system_common.hpp" + +#define GEARBOX_RATIO 40.0 + using namespace perseus_hardware; +using namespace hardware_interface; + +McbSystemHardware::~McbSystemHardware() +{ + // Workaround for ros2_control#472 because they STILL haven't fixed it after over 3 years! + if (_packetManager) + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); +} -hardware_interface::CallbackReturn McbSystemHardware::on_init(const hardware_interface::HardwareInfo& info) +CallbackReturn McbSystemHardware::on_init(const HardwareInfo& info) { - if (const auto& ret = hardware_interface::SystemInterface::on_init(info); ret != hardware_interface::CallbackReturn::SUCCESS) + if (const auto& ret = SystemInterface::on_init(info); + ret != CallbackReturn::SUCCESS) return ret; - return hardware_interface::CallbackReturn::SUCCESS; + CHECK_HARDWARE_PARAMETER_EXISTS(get_logger(), info, "can_bus"); + + for (const auto& joint : info.joints) + { + CHECK_EQ_INTERFACE_COUNT(get_logger(), joint, command_interfaces, "command", 1); + CHECK_EQ_INTERFACE_COUNT(get_logger(), joint, state_interfaces, "state", 4); + + CHECK_INTERFACE_NAME(get_logger(), joint, command_interfaces, "command", 0, HW_IF_VELOCITY); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 0, HW_IF_POSITION); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 1, HW_IF_VELOCITY); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 2, HW_IF_CURRENT); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 3, HW_IF_TEMPERATURE); + + CHECK_PARAMETER_EXISTS(get_logger(), joint, "id"); + try + { + unsigned long escId = std::stoul(joint.parameters.at("id")); + _escIds.emplace_back(escId); + } + catch (const std::exception& e) + { + RCLCPP_FATAL(get_logger(), "Failed to parse ID for joint '%s': %s", joint.name.c_str(), e.what()); + return CallbackReturn::ERROR; + } + + _commandSpeeds.emplace_back(0); + _realPositions.emplace_back(0); + _realSpeeds.emplace_back(0); + _realCurrents.emplace_back(0); + // note: this is in degrees C + _realTemperatures.emplace_back(25); + } + + _lastTransmitError = get_clock()->now(); + + return CallbackReturn::SUCCESS; +} + +std::vector McbSystemHardware::export_state_interfaces() +{ + std::vector stateInterfaces; + const auto& info = get_hardware_info(); + for (size_t i = 0; i < info.joints.size(); i++) + { + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_POSITION, &_realPositions[i])); + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_VELOCITY, &_realSpeeds[i])); + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_CURRENT, &_realCurrents[i])); + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_TEMPERATURE, &_realTemperatures[i])); + } + + return stateInterfaces; } -std::vector McbSystemHardware::export_state_interfaces() +std::vector McbSystemHardware::export_command_interfaces() { - std::vector state_interfaces; - // 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 commandInterfaces; + const auto& info = get_hardware_info(); + for (size_t i = 0; i < info.joints.size(); i++) + { + commandInterfaces.emplace_back(CommandInterface( + info.joints[i].name, HW_IF_VELOCITY, &_commandSpeeds[i])); + } + + return commandInterfaces; } -std::vector McbSystemHardware::export_command_interfaces() +CallbackReturn McbSystemHardware::on_configure( + const rclcpp_lifecycle::State&) { - std::vector command_interfaces; - // 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; + const auto& info = get_hardware_info(); + + try + { + _canInterface.emplace(info.hardware_parameters.at("can_bus")); + _packetManager.emplace(*_canInterface); + } + catch (const std::exception& e) + { + RCLCPP_FATAL(get_logger(), "Failed to initialise CAN bus '%s': %s", + info.hardware_parameters.at("can_bus").c_str(), e.what()); + _packetManager.reset(); + return CallbackReturn::ERROR; + } + + try + { + // We have to reserve the memory to prevent reallocations, + // otherwise the references to the parameter groups will be invalidated + _parameterGroups.reserve(_escIds.size()); + } + catch (std::exception& e) + { + RCLCPP_FATAL(get_logger(), "Failed to reserve memory for ESC parameter groups: %s", e.what()); + return CallbackReturn::ERROR; + } + + for (const auto id : _escIds) + { + try + { + using namespace hi_can; + using namespace addressing::legacy; + + _parameterGroups.emplace_back(address_t( + drive::SYSTEM_ID, + drive::motors::SUBSYSTEM_ID, + id)); + _packetManager->addGroup(_parameterGroups.back()); + } + catch (const std::exception& e) + { + RCLCPP_FATAL(get_logger(), "Failed to set up ESC parameter group %02lu: %s", + id, e.what()); + } + } + + RCLCPP_INFO(get_logger(), "Successfully configured!"); + return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn McbSystemHardware::on_activate( - const rclcpp_lifecycle::State& /*previous_state*/) +CallbackReturn McbSystemHardware::on_cleanup( + const rclcpp_lifecycle::State&) { - // RCLCPP_INFO(get_logger(), "Successfully activated!"); + _packetManager.reset(); + _canInterface.reset(); + _parameterGroups.clear(); + + for (auto& speed : _commandSpeeds) + speed = 0; + for (auto& speed : _realSpeeds) + speed = 0; + for (auto& position : _realPositions) + position = 0; + for (auto& current : _realCurrents) + current = 0; + for (auto& temperature : _realTemperatures) + temperature = 25; - return hardware_interface::CallbackReturn::SUCCESS; + RCLCPP_INFO(get_logger(), "Successfully cleaned up!"); + return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn McbSystemHardware::on_deactivate( - const rclcpp_lifecycle::State& /*previous_state*/) +CallbackReturn McbSystemHardware::on_activate( + const rclcpp_lifecycle::State&) { - // RCLCPP_INFO(get_logger(), "Successfully deactivated!"); + for (auto& group : _parameterGroups) + { + auto& speed = group.getSpeed(); + speed.enabled = true; + speed.direction = hi_can::parameters::legacy::drive::motors::motor_direction::STOP; + speed.speed = 0; + } + RCLCPP_INFO(get_logger(), "Successfully activated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn McbSystemHardware::on_deactivate( + const rclcpp_lifecycle::State&) +{ + for (auto& group : _parameterGroups) + { + auto& speed = group.getSpeed(); + speed.enabled = false; + speed.direction = hi_can::parameters::legacy::drive::motors::motor_direction::STOP; + speed.speed = 0; + } + + try + { + _packetManager->handleTransmit(true); + } + catch (const std::exception& e) + { + RCLCPP_WARN(get_logger(), "Error transmitting motor stop in deactivation: %s", e.what()); + return CallbackReturn::ERROR; + } - return hardware_interface::CallbackReturn::SUCCESS; + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); + return CallbackReturn::SUCCESS; } -hardware_interface::return_type McbSystemHardware::read( - const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) +return_type McbSystemHardware::read( + const rclcpp::Time&, const rclcpp::Duration&) { - return hardware_interface::return_type::OK; + try + { + _packetManager->handleReceive(); + } + catch (const std::exception& e) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Receive failed: %s", e.what()); + } + + for (size_t i = 0; i < _commandSpeeds.size(); i++) + { + auto& paramGroup = _parameterGroups[i]; + const auto& status = paramGroup.getStatus(); + const auto& motorRPM = status.realSpeed; + + // note (and same for write() but in reverse): + // We need to convert from motor RPM to wheel radians per second, + // accounting for the ~1:40 gearbox in between. + // This should probably be handled with a transmission, + // but they're a pain in ros2_control right now. + double wheelRPM = motorRPM / GEARBOX_RATIO; + double revsPerSecond = wheelRPM / 60.0; + double radiansPerSecond = revsPerSecond * (2 * std::numbers::pi); + + _realSpeeds[i] = radiansPerSecond; + _realCurrents[i] = status.realCurrent / 1000.0; // convert from mA to A + } + + return return_type::OK; } -hardware_interface::return_type McbSystemHardware::write( - const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) +return_type McbSystemHardware::write( + const rclcpp::Time&, const rclcpp::Duration&) { - return hardware_interface::return_type::OK; + for (size_t i = 0; i < _commandSpeeds.size(); i++) + { + // see comment in read() on the math + const auto& radiansPerSecond = _commandSpeeds[i]; + double revsPerSecond = radiansPerSecond / (2 * std::numbers::pi); + double wheelRPM = revsPerSecond * 60.0; + double motorRPM = wheelRPM * GEARBOX_RATIO; + + auto& speed = _parameterGroups[i].getSpeed(); + if (abs(motorRPM) < 0.1) + { + speed.direction = hi_can::parameters::legacy::drive::motors::motor_direction::STOP; + } + else if (motorRPM < 0) + speed.direction = hi_can::parameters::legacy::drive::motors::motor_direction::REVERSE; + else + speed.direction = hi_can::parameters::legacy::drive::motors::motor_direction::FORWARD; + + motorRPM = std::abs(motorRPM); + motorRPM = std::min(motorRPM, static_cast(INT16_MAX)); + speed.speed = static_cast(std::round(motorRPM)); + } + + try + { + if (get_clock()->now() - _lastTransmitError > std::chrono::milliseconds(1000)) + _packetManager->handleTransmit(); + } + catch (std::exception& e) + { + RCLCPP_WARN(get_logger(), "Transmit failed: %s", e.what()); + _lastTransmitError = get_clock()->now(); + } + + return return_type::OK; } #include -PLUGINLIB_EXPORT_CLASS(perseus_hardware::McbSystemHardware, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(perseus_hardware::McbSystemHardware, SystemInterface) diff --git a/software/ros_ws/src/perseus_hardware/src/vesc_system.cpp b/software/ros_ws/src/perseus_hardware/src/vesc_system.cpp index f54606e1..28f838f4 100644 --- a/software/ros_ws/src/perseus_hardware/src/vesc_system.cpp +++ b/software/ros_ws/src/perseus_hardware/src/vesc_system.cpp @@ -1,113 +1,272 @@ #include "vesc_system.hpp" +#include +#include #include #include +#include +#include #include #include "system_common.hpp" +#define GEARBOX_RATIO 40.0 + using namespace perseus_hardware; +using namespace hardware_interface; -hardware_interface::CallbackReturn VescSystemHardware::on_init(const hardware_interface::HardwareInfo& info) +VescSystemHardware::~VescSystemHardware() { - if (const auto& ret = hardware_interface::SystemInterface::on_init(info); ret != hardware_interface::CallbackReturn::SUCCESS) - return ret; + // Workaround for ros2_control#472 because they STILL haven't fixed it after over 3 years! + if (_packetManager) + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); +} - _logger = std::make_shared(rclcpp::get_logger("resource_manager.VescSystemHardware")); - _clock = std::make_shared(rclcpp::Clock()); +CallbackReturn VescSystemHardware::on_init(const HardwareInfo& info) +{ + if (const auto& ret = SystemInterface::on_init(info); + ret != CallbackReturn::SUCCESS) + return ret; CHECK_HARDWARE_PARAMETER_EXISTS(get_logger(), info, "can_bus"); for (const auto& joint : info.joints) { - CHECK_INTERFACE_COUNT(get_logger(), joint, command_interfaces, "command", 1); - CHECK_INTERFACE_COUNT(get_logger(), joint, state_interfaces, "state", 2); + CHECK_EQ_INTERFACE_COUNT(get_logger(), joint, command_interfaces, "command", 1); + CHECK_EQ_INTERFACE_COUNT(get_logger(), joint, state_interfaces, "state", 4); - CHECK_INTERFACE_NAME(get_logger(), joint, command_interfaces, "command", 0, hardware_interface::HW_IF_VELOCITY); - CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 0, hardware_interface::HW_IF_POSITION); - CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 1, hardware_interface::HW_IF_VELOCITY); + CHECK_INTERFACE_NAME(get_logger(), joint, command_interfaces, "command", 0, HW_IF_VELOCITY); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 0, HW_IF_POSITION); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 1, HW_IF_VELOCITY); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 2, HW_IF_CURRENT); + CHECK_INTERFACE_NAME(get_logger(), joint, state_interfaces, "state", 3, HW_IF_TEMPERATURE); CHECK_PARAMETER_EXISTS(get_logger(), joint, "id"); - // unsigned long vescId = std::stoul(joint.parameters.at("id")); + try + { + unsigned long vescId = std::stoul(joint.parameters.at("id")); + _vescIds.emplace_back(vescId); + } + catch (const std::exception& e) + { + RCLCPP_FATAL(get_logger(), "Failed to parse ID for joint '%s': %s", joint.name.c_str(), e.what()); + return CallbackReturn::ERROR; + } _commandSpeeds.emplace_back(0); _realPositions.emplace_back(0); _realSpeeds.emplace_back(0); + _realCurrents.emplace_back(0); + // note: this is in degrees C + _realTemperatures.emplace_back(25); } - return hardware_interface::CallbackReturn::SUCCESS; + _lastTransmitError = get_clock()->now(); + + return CallbackReturn::SUCCESS; } -std::vector VescSystemHardware::export_state_interfaces() +std::vector VescSystemHardware::export_state_interfaces() { - std::vector state_interfaces; - const auto& info = get_info(); + std::vector stateInterfaces; + const auto& info = get_hardware_info(); for (size_t i = 0; i < info.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info.joints[i].name, hardware_interface::HW_IF_POSITION, &_realPositions[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &_realSpeeds[i])); + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_POSITION, &_realPositions[i])); + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_VELOCITY, &_realSpeeds[i])); + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_CURRENT, &_realCurrents[i])); + stateInterfaces.emplace_back(StateInterface( + info.joints[i].name, HW_IF_TEMPERATURE, &_realTemperatures[i])); } - return state_interfaces; + return stateInterfaces; } -std::vector VescSystemHardware::export_command_interfaces() +std::vector VescSystemHardware::export_command_interfaces() { - std::vector command_interfaces; - const auto& info = get_info(); + std::vector commandInterfaces; + const auto& info = get_hardware_info(); for (size_t i = 0; i < info.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &_commandSpeeds[i])); + commandInterfaces.emplace_back(CommandInterface( + info.joints[i].name, HW_IF_VELOCITY, &_commandSpeeds[i])); } - return command_interfaces; + return commandInterfaces; } -hardware_interface::CallbackReturn VescSystemHardware::on_activate( - const rclcpp_lifecycle::State& /*previous_state*/) +CallbackReturn VescSystemHardware::on_configure( + const rclcpp_lifecycle::State&) { - const auto& info = get_info(); + const auto& info = get_hardware_info(); + try { - hi_can::RawCanInterface canInterface(info.hardware_parameters.at("can_bus")); - _packetManager.emplace(canInterface); + _canInterface.emplace(info.hardware_parameters.at("can_bus")); + _packetManager.emplace(*_canInterface); } catch (const std::exception& e) { - RCLCPP_FATAL(get_logger(), "Failed to initialise CAN bus (%s): %s", + RCLCPP_FATAL(get_logger(), "Failed to initialise CAN bus '%s': %s", info.hardware_parameters.at("can_bus").c_str(), e.what()); - return hardware_interface::CallbackReturn::ERROR; + _packetManager.reset(); + return CallbackReturn::ERROR; + } + + try + { + // We have to reserve the memory to prevent reallocations, + // otherwise the references to the parameter groups will be invalidated + _parameterGroups.reserve(_vescIds.size()); + } + catch (std::exception& e) + { + RCLCPP_FATAL(get_logger(), "Failed to reserve memory for ESC parameter groups: %s", e.what()); + return CallbackReturn::ERROR; + } + + for (const auto id : _vescIds) + { + try + { + using namespace hi_can; + using namespace addressing::drive::vesc; + + _parameterGroups.emplace_back(id); + _packetManager->addGroup(_parameterGroups.back()); + } + catch (const std::exception& e) + { + RCLCPP_FATAL(get_logger(), "Failed to set up ESC parameter group %02lu: %s", + id, e.what()); + } } - // RCLCPP_INFO(get_logger(), "Successfully activated!"); - return hardware_interface::CallbackReturn::SUCCESS; + RCLCPP_INFO(get_logger(), "Successfully configured!"); + return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn VescSystemHardware::on_deactivate( - const rclcpp_lifecycle::State& /*previous_state*/) +CallbackReturn VescSystemHardware::on_cleanup( + const rclcpp_lifecycle::State&) { _packetManager.reset(); + _canInterface.reset(); + _parameterGroups.clear(); + + for (auto& speed : _commandSpeeds) + speed = 0; + for (auto& speed : _realSpeeds) + speed = 0; + for (auto& position : _realPositions) + position = 0; + for (auto& current : _realCurrents) + current = 0; + for (auto& temperature : _realTemperatures) + temperature = 25; - // RCLCPP_INFO(get_logger(), "Successfully deactivated!"); - return hardware_interface::CallbackReturn::SUCCESS; + RCLCPP_INFO(get_logger(), "Successfully cleaned up!"); + return CallbackReturn::SUCCESS; } -hardware_interface::return_type VescSystemHardware::read( - const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) +CallbackReturn VescSystemHardware::on_activate( + const rclcpp_lifecycle::State&) { - _packetManager->handleReceive(); - return hardware_interface::return_type::OK; + // No enable line or similar applicable to VESCs + RCLCPP_INFO(get_logger(), "Successfully activated!"); + return CallbackReturn::SUCCESS; } -hardware_interface::return_type VescSystemHardware::write( - const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) +CallbackReturn VescSystemHardware::on_deactivate( + const rclcpp_lifecycle::State&) { - _packetManager->handleTransmit(); - return hardware_interface::return_type::OK; + for (auto& group : _parameterGroups) + group.getSetRpm().value = 0; + + try + { + _packetManager->handleTransmit(true); + } + catch (const std::exception& e) + { + RCLCPP_WARN(get_logger(), "Error transmitting motor stop in deactivation: %s", e.what()); + return CallbackReturn::ERROR; + } + + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); + return CallbackReturn::SUCCESS; +} + +return_type VescSystemHardware::read( + const rclcpp::Time&, const rclcpp::Duration&) +{ + try + { + _packetManager->handleReceive(); + } + catch (const std::exception& e) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Receive failed: %s", e.what()); + } + + for (size_t i = 0; i < _commandSpeeds.size(); i++) + { + auto& paramGroup = _parameterGroups[i]; + const auto& status = paramGroup.getStatus1(); + const auto& motorRPM = status.rpm; + + // note (and same for write() but in reverse): + // We need to convert from motor RPM to wheel radians per second, + // accounting for the ~1:40 gearbox in between. + // This should probably be handled with a transmission, + // but they're a pain in ros2_control right now. + double wheelRPM = motorRPM / GEARBOX_RATIO; + double revsPerSecond = wheelRPM / 60.0; + double radiansPerSecond = revsPerSecond * (2 * std::numbers::pi); + + _realSpeeds[i] = radiansPerSecond; + _realCurrents[i] = status.current; + + const auto& status4 = paramGroup.getStatus4(); + // TODO: Test to see which would be more appropriate to use + // (probably motor temp) + _realTemperatures[i] = std::max(status4.tempFet, status4.tempMotor); + } + + return return_type::OK; +} + +return_type VescSystemHardware::write( + const rclcpp::Time&, const rclcpp::Duration&) +{ + for (size_t i = 0; i < _commandSpeeds.size(); i++) + { + // see comment in read() on the math + const auto& radiansPerSecond = _commandSpeeds[i]; + double revsPerSecond = radiansPerSecond / (2 * std::numbers::pi); + double wheelRPM = revsPerSecond * 60.0; + double motorRPM = wheelRPM * GEARBOX_RATIO; + + motorRPM = std::clamp(motorRPM, static_cast(INT32_MIN), static_cast(INT32_MAX)); + _parameterGroups[i].getSetRpm().value = static_cast(std::round(motorRPM)); + } + + try + { + if (get_clock()->now() - _lastTransmitError > std::chrono::milliseconds(1000)) + _packetManager->handleTransmit(); + } + catch (std::exception& e) + { + RCLCPP_WARN(get_logger(), "Transmit failed: %s", e.what()); + _lastTransmitError = get_clock()->now(); + } + + return return_type::OK; } #include -PLUGINLIB_EXPORT_CLASS(perseus_hardware::VescSystemHardware, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(perseus_hardware::VescSystemHardware, SystemInterface)