From 6b289d923e0b5ddc665fbe036d0f7099e1650222 Mon Sep 17 00:00:00 2001 From: Chen Chen Date: Tue, 12 Dec 2023 16:06:11 +0800 Subject: [PATCH] Ensure device is properly stopped - add: destructor in FDEffortHardwareInterface - add: call on_deactivate if controller manager is shutdown via ctrl+c --- fd_hardware/include/fd_hardware/fd_effort_hi.hpp | 2 ++ fd_hardware/src/fd_effort_hi.cpp | 8 ++++++++ 2 files changed, 10 insertions(+) diff --git a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp index 4186b93..6fd65cc 100644 --- a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp +++ b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp @@ -35,6 +35,8 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface public: RCLCPP_SHARED_PTR_DEFINITIONS(FDEffortHardwareInterface); + virtual ~FDEffortHardwareInterface(); + FD_HARDWARE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index 8c8b2f7..faf0ea5 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -35,6 +35,14 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface namespace fd_hardware { + +FDEffortHardwareInterface::~FDEffortHardwareInterface() +{ + // If controller manager is shutdown via Ctrl + C, the on_deactivate methods won't be called. + // We need to call them here to ensure that the device is stopped and disconnected. + on_deactivate(rclcpp_lifecycle::State()); +} + // ------------------------------------------------------------------------------------------ CallbackReturn FDEffortHardwareInterface::on_init( const hardware_interface::HardwareInfo & info)