From e234a93e03dede715f1d4a0fa3240cc370d57a91 Mon Sep 17 00:00:00 2001 From: gerry Date: Thu, 28 Sep 2023 15:03:33 -0700 Subject: [PATCH] Add services to disable/enable motor so that brake is usable. --- .../include/canopen_402_driver/motor.hpp | 17 +++++++ .../node_canopen_402_driver.hpp | 28 +++++++++++ .../node_canopen_402_driver_impl.hpp | 49 +++++++++++++++++++ canopen_402_driver/src/motor.cpp | 33 +++++++++++++ 4 files changed, 127 insertions(+) diff --git a/canopen_402_driver/include/canopen_402_driver/motor.hpp b/canopen_402_driver/include/canopen_402_driver/motor.hpp index cf040f97..d0b40d1a 100644 --- a/canopen_402_driver/include/canopen_402_driver/motor.hpp +++ b/canopen_402_driver/include/canopen_402_driver/motor.hpp @@ -130,6 +130,23 @@ class Motor402 : public MotorBase */ bool handleRecover(); + /** + * @brief Enable the drive + * + * This function enables the drive. This means it attempts + * to bring the device to operational state (CIA402), and does nothing else. + * + */ + bool handleEnable(); + /** + * @brief Disable the drive + * + * This function disables the drive. This means it attempts to bring the + * device to switched on disabled state (CIA402). + * + */ + bool handleDisable(); + /** * @brief Register a new operation mode for the drive * diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp index 1e38588e..5274820a 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp @@ -42,6 +42,8 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver std::shared_ptr motor_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Service::SharedPtr handle_init_service; + rclcpp::Service::SharedPtr handle_enable_service; + rclcpp::Service::SharedPtr handle_disable_service; rclcpp::Service::SharedPtr handle_halt_service; rclcpp::Service::SharedPtr handle_recover_service; rclcpp::Service::SharedPtr handle_set_mode_position_service; @@ -90,6 +92,32 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response); + /** + * @brief Service Callback to enable device + * + * Calls Motor402::handleEnable function. Brings motor to enabled + * state. + * + * @param [in] request + * @param [out] response + */ + void handle_enable( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Service Callback to disable device + * + * Calls Motor402::handleDisable function. Brings motor to switched on + * disabled state. + * + * @param [in] request + * @param [out] response + */ + void handle_disable( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + /** * @brief Method to initialise device * diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index 48cc5e1b..9edf1141 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -50,6 +50,18 @@ void NodeCanopen402Driver::init(bool called_from_base) std::bind( &NodeCanopen402Driver::handle_init, this, std::placeholders::_1, std::placeholders::_2)); + + handle_enable_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/enable").c_str(), + std::bind( + &NodeCanopen402Driver::handle_enable, this, std::placeholders::_1, + std::placeholders::_2)); + + handle_disable_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/disable").c_str(), + std::bind( + &NodeCanopen402Driver::handle_disable, this, std::placeholders::_1, + std::placeholders::_2)); handle_halt_service = this->node_->create_service( std::string(this->node_->get_name()).append("/halt").c_str(), @@ -119,6 +131,18 @@ void NodeCanopen402Driver::init(bool called_fro &NodeCanopen402Driver::handle_init, this, std::placeholders::_1, std::placeholders::_2)); + handle_enable_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/enable").c_str(), + std::bind( + &NodeCanopen402Driver::handle_enable, this, + std::placeholders::_1, std::placeholders::_2)); + + handle_disable_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/disable").c_str(), + std::bind( + &NodeCanopen402Driver::handle_disable, this, + std::placeholders::_1, std::placeholders::_2)); + handle_halt_service = this->node_->create_service( std::string(this->node_->get_name()).append("/halt").c_str(), std::bind( @@ -443,6 +467,31 @@ void NodeCanopen402Driver::handle_set_target( } } +template +void NodeCanopen402Driver::handle_disable( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (this->activated_.load()) + { + bool temp = motor_->handleDisable(); + response->success = temp; + } +} + +template +void NodeCanopen402Driver::handle_enable( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (this->activated_.load()) + { + bool temp = motor_->handleEnable(); + response->success = temp; + } +} + + template bool NodeCanopen402Driver::init_motor() { diff --git a/canopen_402_driver/src/motor.cpp b/canopen_402_driver/src/motor.cpp index 28adfa02..ef71e8fc 100644 --- a/canopen_402_driver/src/motor.cpp +++ b/canopen_402_driver/src/motor.cpp @@ -453,3 +453,36 @@ bool Motor402::handleRecover() } return true; } +bool Motor402::handleEnable() +{ + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Enable: Read State"); + if (!readState()) + { + std::cout << "Could not read motor state" << std::endl; + return false; + } + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Enable"); + if (!switchState(State402::Operation_Enable)) + { + std::cout << "Could not enable motor" << std::endl; + return false; + } + return true; +} + +bool Motor402::handleDisable() +{ + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Disable: Read State"); + if (!readState()) + { + std::cout << "Could not read motor state" << std::endl; + return false; + } + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Disable"); + if (!switchState(State402::Switched_On)) + { + std::cout << "Could not disable motor" << std::endl; + return false; + } + return true; +}