Fix issue with lifecycle

Author: pasztork

kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface_eki_rsi.hpp

@@ -64,6 +64,8 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface
 
   KUKA_KSS_RSI_DRIVER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
 
+  KUKA_KSS_RSI_DRIVER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
+
   KUKA_KSS_RSI_DRIVER_PUBLIC
   CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
 

kuka_kss_rsi_driver/src/hardware_interface_eki_rsi.cpp

@@ -95,6 +95,12 @@ CallbackReturn KukaRSIHardwareInterface::on_configure(const rclcpp_lifecycle::St
   return setup_success ? CallbackReturn::SUCCESS : CallbackReturn::ERROR;
 }
 
+CallbackReturn KukaRSIHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  robot_ptr_.reset();
+  return CallbackReturn::SUCCESS;
+}
+
 CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
 {
   const auto control_mode =
@@ -124,7 +130,6 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta
 
 CallbackReturn KukaRSIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
 {
-  RCLCPP_INFO(logger_, "Deactivating hardware interface");
   set_stop_flag();
   return CallbackReturn::SUCCESS;
 }
@@ -234,10 +239,12 @@ void KukaRSIHardwareInterface::Write()
   if (stop_requested_)
   {
     RCLCPP_INFO(logger_, "Sending stop signal");
+    is_active_ = false;
     send_reply_status = robot_ptr_->StopControlling();
   }
   else if (control_mode_change_requested)
   {
+    // TODO(pasztork): Test this branch once other control modes become available.
     RCLCPP_INFO(logger_, "Requesting control mode change");
     send_reply_status = robot_ptr_->SwitchControlMode(
       static_cast<kuka::external::control::ControlMode>(hw_control_mode_command_));

kuka_kss_rsi_driver/src/robot_manager_node_eki_rsi.cpp

@@ -75,8 +75,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_
     [this](const std::string & robot_model) { return OnRobotModelChangeRequest(robot_model); });
 }
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
+CallbackReturn RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
 {
   const auto logger = get_logger();
 
@@ -115,8 +114,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
   return SUCCESS;
 }
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)
+CallbackReturn RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)
 {
   const auto logger = get_logger();
 
@@ -149,8 +147,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)
   return SUCCESS;
 }
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
+CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
 {
   const auto logger = get_logger();
   terminate_ = false;
@@ -187,8 +184,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
   return SUCCESS;
 }
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
+CallbackReturn RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
 {
   const auto logger = get_logger();