Enable/disable services

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
    *

canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp

@@ -42,6 +42,8 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
   std::shared_ptr<Motor402> motor_;
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_init_service;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_enable_service;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_disable_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_halt_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_recover_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_position_service;
@@ -91,6 +93,32 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
     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
    *

canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp

@@ -51,6 +51,18 @@ void NodeCanopen402Driver<rclcpp::Node>::init(bool called_from_base)
       &NodeCanopen402Driver<rclcpp::Node>::handle_init, this, std::placeholders::_1,
       std::placeholders::_2));
 
+  handle_enable_service = this->node_->create_service<std_srvs::srv::Trigger>(
+    std::string(this->node_->get_name()).append("/enable").c_str(),
+    std::bind(
+      &NodeCanopen402Driver<rclcpp::Node>::handle_enable, this, std::placeholders::_1,
+      std::placeholders::_2));
+
+  handle_disable_service = this->node_->create_service<std_srvs::srv::Trigger>(
+    std::string(this->node_->get_name()).append("/disable").c_str(),
+    std::bind(
+      &NodeCanopen402Driver<rclcpp::Node>::handle_disable, this, std::placeholders::_1,
+      std::placeholders::_2));
+
   handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
     std::string(this->node_->get_name()).append("/halt").c_str(),
     std::bind(
@@ -125,6 +137,18 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::init(bool called_fro
       &NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_init, this,
       std::placeholders::_1, std::placeholders::_2));
 
+  handle_enable_service = this->node_->create_service<std_srvs::srv::Trigger>(
+    std::string(this->node_->get_name()).append("/enable").c_str(),
+    std::bind(
+      &NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_enable, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  handle_disable_service = this->node_->create_service<std_srvs::srv::Trigger>(
+    std::string(this->node_->get_name()).append("/disable").c_str(),
+    std::bind(
+      &NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_disable, this,
+      std::placeholders::_1, std::placeholders::_2));
+
   handle_halt_service = this->node_->create_service<std_srvs::srv::Trigger>(
     std::string(this->node_->get_name()).append("/halt").c_str(),
     std::bind(
@@ -463,6 +487,30 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_target(
   }
 }
 
+template <class NODETYPE>
+void NodeCanopen402Driver<NODETYPE>::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 <class NODETYPE>
+void NodeCanopen402Driver<NODETYPE>::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 <class NODETYPE>
 bool NodeCanopen402Driver<NODETYPE>::init_motor()
 {

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;
+}