diff --git a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp
index 8a6505da..aa2ad56f 100644
--- a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp
+++ b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp
@@ -81,23 +81,6 @@ class Cia402Driver : public ros2_canopen::CanopenDriver
 
   bool halt_motor() { return node_canopen_402_driver_->halt_motor(); }
 
-  bool set_mode_position() { return node_canopen_402_driver_->set_mode_position(); }
-
-  bool set_mode_velocity() { return node_canopen_402_driver_->set_mode_velocity(); }
-
-  bool set_mode_cyclic_position() { return node_canopen_402_driver_->set_mode_cyclic_position(); }
-
-  bool set_mode_cyclic_velocity() { return node_canopen_402_driver_->set_mode_cyclic_velocity(); }
-
-  bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); }
-
-  bool set_mode_cyclic_torque() { return node_canopen_402_driver_->set_mode_cyclic_torque(); }
-
-  bool set_mode_interpolated_position()
-  {
-    return node_canopen_402_driver_->set_mode_interpolated_position();
-  }
-
   uint16_t get_mode() { return node_canopen_402_driver_->get_mode(); }
 
   bool set_operation_mode(uint16_t mode)
diff --git a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp
index 4d1009f0..4f79adac 100644
--- a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp
+++ b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp
@@ -83,21 +83,11 @@ class LifecycleCia402Driver : public ros2_canopen::LifecycleCanopenDriver
 
   bool halt_motor() { return node_canopen_402_driver_->halt_motor(); }
 
-  bool set_mode_position() { return node_canopen_402_driver_->set_mode_position(); }
+  uint16_t get_mode() { return node_canopen_402_driver_->get_mode(); }
 
-  bool set_mode_velocity() { return node_canopen_402_driver_->set_mode_velocity(); }
-
-  bool set_mode_cyclic_position() { return node_canopen_402_driver_->set_mode_cyclic_position(); }
-
-  bool set_mode_cyclic_velocity() { return node_canopen_402_driver_->set_mode_cyclic_velocity(); }
-
-  bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); }
-
-  bool set_mode_cyclic_torque() { return node_canopen_402_driver_->set_mode_cyclic_torque(); }
-
-  bool set_mode_interpolated_position()
+  bool set_operation_mode(uint16_t mode)
   {
-    return node_canopen_402_driver_->set_mode_interpolated_position();
+    return node_canopen_402_driver_->set_operation_mode(mode);
   }
 };
 }  // namespace ros2_canopen
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 13a01a01..36f31741 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
@@ -157,33 +157,30 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
   bool halt_motor();
 
   /**
-   * @brief Service Callback to set profiled position mode
+   * @brief Service Callback to set operation mode
    *
-   * Calls Motor402::enterModeAndWait with Profiled Position Mode as
-   * Target Operation Mode. If successful, the motor was transitioned
-   * to Profiled Position Mode.
+   * Calls Motor402::enterModeAndWait with requested Operation Mode.
    *
-   * @param [in] request
+   * @param [in] request Requested Operation Mode as MotorBase::Profiled_Position or
+   * MotorBase::Profiled_Velocity or MotorBase::Profiled_Torque or MotorBase::Cyclic_Position or
+   * MotorBase::Cyclic_Velocity or MotorBase::Cyclic_Torque or MotorBase::Interpolated_Position
    * @param [out] response
    */
-  void handle_set_mode_position(
-    const std_srvs::srv::Trigger::Request::SharedPtr request,
-    std_srvs::srv::Trigger::Response::SharedPtr response);
-
   bool set_operation_mode(uint16_t mode);
 
   /**
-   * @brief Method to set profiled position mode
+   * @brief Service Callback to set profiled position mode
    *
    * Calls Motor402::enterModeAndWait with Profiled Position Mode as
    * Target Operation Mode. If successful, the motor was transitioned
    * to Profiled Position Mode.
    *
-   * @param [in] void
-   *
-   * @return bool
+   * @param [in] request
+   * @param [out] response
    */
-  bool set_mode_position();
+  void handle_set_mode_position(
+    const std_srvs::srv::Trigger::Request::SharedPtr request,
+    std_srvs::srv::Trigger::Response::SharedPtr response);
 
   /**
    * @brief Service Callback to set profiled velocity mode
@@ -199,19 +196,6 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
     const std_srvs::srv::Trigger::Request::SharedPtr request,
     std_srvs::srv::Trigger::Response::SharedPtr response);
 
-  /**
-   * @brief Method to set profiled velocity mode
-   *
-   * Calls Motor402::enterModeAndWait with Profiled Velocity Mode as
-   * Target Operation Mode. If successful, the motor was transitioned
-   * to Profiled Velocity Mode.
-   *
-   * @param [in] void
-   *
-   * @return bool
-   */
-  bool set_mode_velocity();
-
   /**
    * @brief Service Callback to set cyclic position mode
    *
@@ -240,31 +224,6 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
     const std_srvs::srv::Trigger::Request::SharedPtr request,
     std_srvs::srv::Trigger::Response::SharedPtr response);
 
-  /**
-   * @brief Method to set interpolated position mode
-   *
-   * Calls Motor402::enterModeAndWait with Interpolated Position Mode as
-   * Target Operation Mode. If successful, the motor was transitioned
-   * to Interpolated Position Mode. This only supports linear mode.
-   *
-   * @param [in] void
-   * @param [out] bool
-   */
-  bool set_mode_interpolated_position();
-
-  /**
-   * @brief Method to set cyclic position mode
-   *
-   * Calls Motor402::enterModeAndWait with Cyclic Position Mode as
-   * Target Operation Mode. If successful, the motor was transitioned
-   * to Cyclic Position Mode.
-   *
-   * @param [in] void
-   *
-   * @return bool
-   */
-  bool set_mode_cyclic_position();
-
   /**
    * @brief Service Callback to set cyclic velocity mode
    *
@@ -279,19 +238,6 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
     const std_srvs::srv::Trigger::Request::SharedPtr request,
     std_srvs::srv::Trigger::Response::SharedPtr response);
 
-  /**
-   * @brief Method to set cyclic velocity mode
-   *
-   * Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as
-   * Target Operation Mode. If successful, the motor was transitioned
-   * to Cyclic Velocity Mode.
-   *
-   * @param [in] void
-   *
-   * @return bool
-   */
-  bool set_mode_cyclic_velocity();
-
   /**
    * @brief Service Callback to set profiled torque mode
    *
@@ -306,19 +252,6 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
     const std_srvs::srv::Trigger::Request::SharedPtr request,
     std_srvs::srv::Trigger::Response::SharedPtr response);
 
-  /**
-   * @brief Method to set profiled torque mode
-   *
-   * Calls Motor402::enterModeAndWait with Profiled Torque Mode as
-   * Target Operation Mode. If successful, the motor was transitioned
-   * to Profiled Torque Mode.
-   *
-   * @param [in] void
-   *
-   * @return bool
-   */
-  bool set_mode_torque();
-
   /**
    * @brief Service Callback to set cyclic torque mode
    *
@@ -333,19 +266,6 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
     const std_srvs::srv::Trigger::Request::SharedPtr request,
     std_srvs::srv::Trigger::Response::SharedPtr response);
 
-  /**
-   * @brief Method to set cyclic torque mode
-   *
-   * Calls Motor402::enterModeAndWait with Profiled Torque Mode as
-   * Target Operation Mode. If successful, the motor was transitioned
-   * to Profiled Torque Mode.
-   *
-   * @param [in] void
-   *
-   * @return bool
-   */
-  bool set_mode_cyclic_torque();
-
   /**
    * @brief Service Callback to set target
    *
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 0df26964..fcd2d35d 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
@@ -383,7 +383,7 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_mode_position(
   const std_srvs::srv::Trigger::Request::SharedPtr request,
   std_srvs::srv::Trigger::Response::SharedPtr response)
 {
-  response->success = set_mode_position();
+  response->success = set_operation_mode(MotorBase::Profiled_Position);
 }
 
 template <class NODETYPE>
@@ -391,7 +391,7 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_mode_velocity(
   const std_srvs::srv::Trigger::Request::SharedPtr request,
   std_srvs::srv::Trigger::Response::SharedPtr response)
 {
-  response->success = set_mode_velocity();
+  response->success = set_operation_mode(MotorBase::Profiled_Velocity);
 }
 
 template <class NODETYPE>
@@ -399,7 +399,7 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_mode_cyclic_position(
   const std_srvs::srv::Trigger::Request::SharedPtr request,
   std_srvs::srv::Trigger::Response::SharedPtr response)
 {
-  response->success = set_mode_cyclic_position();
+  response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Position);
 }
 
 template <class NODETYPE>
@@ -407,7 +407,7 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_mode_interpolated_position(
   const std_srvs::srv::Trigger::Request::SharedPtr request,
   std_srvs::srv::Trigger::Response::SharedPtr response)
 {
-  response->success = set_mode_interpolated_position();
+  response->success = set_operation_mode(MotorBase::Interpolated_Position);
 }
 
 template <class NODETYPE>
@@ -415,14 +415,14 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_mode_cyclic_velocity(
   const std_srvs::srv::Trigger::Request::SharedPtr request,
   std_srvs::srv::Trigger::Response::SharedPtr response)
 {
-  response->success = set_mode_cyclic_velocity();
+  response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Velocity);
 }
 template <class NODETYPE>
 void NodeCanopen402Driver<NODETYPE>::handle_set_mode_torque(
   const std_srvs::srv::Trigger::Request::SharedPtr request,
   std_srvs::srv::Trigger::Response::SharedPtr response)
 {
-  response->success = set_mode_torque();
+  response->success = set_operation_mode(MotorBase::Profiled_Torque);
 }
 
 template <class NODETYPE>
@@ -430,7 +430,7 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_mode_cyclic_torque(
   const std_srvs::srv::Trigger::Request::SharedPtr request,
   std_srvs::srv::Trigger::Response::SharedPtr response)
 {
-  response->success = set_mode_cyclic_torque();
+  response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Torque);
 }
 
 template <class NODETYPE>
@@ -509,149 +509,16 @@ bool NodeCanopen402Driver<NODETYPE>::set_operation_mode(uint16_t mode)
 {
   if (this->activated_.load())
   {
-    return motor_->enterModeAndWait(mode);
-  }
-  return false;
-}
-
-template <class NODETYPE>
-bool NodeCanopen402Driver<NODETYPE>::set_mode_position()
-{
-  if (this->activated_.load())
-  {
-    if (motor_->getMode() != MotorBase::Profiled_Position)
-    {
-      return motor_->enterModeAndWait(MotorBase::Profiled_Position);
-    }
-    else
-    {
-      return false;
-    }
-  }
-  else
-  {
-    return false;
-  }
-}
-
-template <class NODETYPE>
-bool NodeCanopen402Driver<NODETYPE>::set_mode_interpolated_position()
-{
-  if (this->activated_.load())
-  {
-    if (motor_->getMode() != MotorBase::Interpolated_Position)
-    {
-      return motor_->enterModeAndWait(MotorBase::Interpolated_Position);
-    }
-    else
-    {
-      return false;
-    }
-  }
-  else
-  {
-    return false;
-  }
-}
-
-template <class NODETYPE>
-bool NodeCanopen402Driver<NODETYPE>::set_mode_velocity()
-{
-  if (this->activated_.load())
-  {
-    if (motor_->getMode() != MotorBase::Profiled_Velocity)
+    if (motor_->getMode() != mode)
     {
-      return motor_->enterModeAndWait(MotorBase::Profiled_Velocity);
+      return motor_->enterModeAndWait(mode);
     }
     else
     {
       return false;
     }
   }
-  else
-  {
-    return false;
-  }
-}
-
-template <class NODETYPE>
-bool NodeCanopen402Driver<NODETYPE>::set_mode_cyclic_position()
-{
-  if (this->activated_.load())
-  {
-    if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Position)
-    {
-      return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Position);
-    }
-    else
-    {
-      return false;
-    }
-  }
-  else
-  {
-    return false;
-  }
-}
-
-template <class NODETYPE>
-bool NodeCanopen402Driver<NODETYPE>::set_mode_cyclic_velocity()
-{
-  if (this->activated_.load())
-  {
-    if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Velocity)
-    {
-      return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Velocity);
-    }
-    else
-    {
-      return false;
-    }
-  }
-  else
-  {
-    return false;
-  }
-}
-
-template <class NODETYPE>
-bool NodeCanopen402Driver<NODETYPE>::set_mode_cyclic_torque()
-{
-  if (this->activated_.load())
-  {
-    if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Torque)
-    {
-      return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Torque);
-    }
-    else
-    {
-      return false;
-    }
-  }
-  else
-  {
-    return false;
-  }
-}
-
-template <class NODETYPE>
-bool NodeCanopen402Driver<NODETYPE>::set_mode_torque()
-{
-  if (this->activated_.load())
-  {
-    if (motor_->getMode() != MotorBase::Profiled_Torque)
-    {
-      return motor_->enterModeAndWait(MotorBase::Profiled_Torque);
-    }
-    else
-    {
-      return false;
-    }
-  }
-  else
-  {
-    return false;
-  }
+  return false;
 }
 
 template <class NODETYPE>
diff --git a/canopen_ros2_control/src/cia402_system.cpp b/canopen_ros2_control/src/cia402_system.cpp
index 48b4b99b..8c0cc6f2 100644
--- a/canopen_ros2_control/src/cia402_system.cpp
+++ b/canopen_ros2_control/src/cia402_system.cpp
@@ -330,33 +330,38 @@ void Cia402System::switchModes(uint id, const std::shared_ptr<ros2_canopen::Cia4
 {
   if (motor_data_[id].position_mode.is_commanded())
   {
-    motor_data_[id].position_mode.set_response(driver->set_mode_position());
+    motor_data_[id].position_mode.set_response(
+      driver->set_operation_mode(MotorBase::Profiled_Position));
   }
 
   if (motor_data_[id].cyclic_position_mode.is_commanded())
   {
-    motor_data_[id].cyclic_position_mode.set_response(driver->set_mode_cyclic_position());
+    motor_data_[id].cyclic_position_mode.set_response(
+      driver->set_operation_mode(MotorBase::Cyclic_Synchronous_Position));
   }
 
   if (motor_data_[id].velocity_mode.is_commanded())
   {
-    motor_data_[id].velocity_mode.set_response(driver->set_mode_velocity());
+    motor_data_[id].velocity_mode.set_response(
+      driver->set_operation_mode(MotorBase::Profiled_Velocity));
   }
 
   if (motor_data_[id].cyclic_velocity_mode.is_commanded())
   {
-    motor_data_[id].cyclic_velocity_mode.set_response(driver->set_mode_cyclic_velocity());
+    motor_data_[id].cyclic_velocity_mode.set_response(
+      driver->set_operation_mode(MotorBase::Cyclic_Synchronous_Velocity));
   }
 
   if (motor_data_[id].torque_mode.is_commanded())
   {
-    motor_data_[id].torque_mode.set_response(driver->set_mode_torque());
+    motor_data_[id].torque_mode.set_response(
+      driver->set_operation_mode(MotorBase::Profiled_Torque));
   }
 
   if (motor_data_[id].interpolated_position_mode.is_commanded())
   {
     motor_data_[id].interpolated_position_mode.set_response(
-      driver->set_mode_interpolated_position());
+      driver->set_operation_mode(MotorBase::Interpolated_Position));
   }
 }