Moved registering publisher and service to on_active (#151)

* Reset publisher and service in on_deactive

* Added a function to call service in the test

Author: urmahp

ur_controllers/src/gpio_controller.cpp

@@ -123,29 +123,6 @@ controller_interface::return_type ur_controllers::GPIOController::update()
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 ur_controllers::GPIOController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
 {
-  try {
-    // register publisher
-    io_pub_ = get_node()->create_publisher<ur_msgs::msg::IOStates>("~/io_states", rclcpp::SystemDefaultsQoS());
-
-    tool_data_pub_ =
-        get_node()->create_publisher<ur_msgs::msg::ToolDataMsg>("~/tool_data", rclcpp::SystemDefaultsQoS());
-
-    robot_mode_pub_ =
-        get_node()->create_publisher<ur_dashboard_msgs::msg::RobotMode>("~/robot_mode", rclcpp::SystemDefaultsQoS());
-
-    safety_mode_pub_ =
-        get_node()->create_publisher<ur_dashboard_msgs::msg::SafetyMode>("~/safety_mode", rclcpp::SystemDefaultsQoS());
-
-    set_io_srv_ = get_node()->create_service<ur_msgs::srv::SetIO>(
-        "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2));
-
-    set_speed_slider_srv_ = get_node()->create_service<ur_msgs::srv::SetSpeedSliderFraction>(
-        "~/set_speed_slider",
-        std::bind(&GPIOController::setSpeedSlider, this, std::placeholders::_1, std::placeholders::_2));
-  } catch (...) {
-    return LifecycleNodeInterface::CallbackReturn::ERROR;
-  }
-
   return LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
@@ -220,12 +197,45 @@ void GPIOController::publishSafetyMode()
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
+  try {
+    // register publisher
+    io_pub_ = get_node()->create_publisher<ur_msgs::msg::IOStates>("~/io_states", rclcpp::SystemDefaultsQoS());
+
+    tool_data_pub_ =
+        get_node()->create_publisher<ur_msgs::msg::ToolDataMsg>("~/tool_data", rclcpp::SystemDefaultsQoS());
+
+    robot_mode_pub_ =
+        get_node()->create_publisher<ur_dashboard_msgs::msg::RobotMode>("~/robot_mode", rclcpp::SystemDefaultsQoS());
+
+    safety_mode_pub_ =
+        get_node()->create_publisher<ur_dashboard_msgs::msg::SafetyMode>("~/safety_mode", rclcpp::SystemDefaultsQoS());
+
+    set_io_srv_ = get_node()->create_service<ur_msgs::srv::SetIO>(
+        "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2));
+
+    set_speed_slider_srv_ = get_node()->create_service<ur_msgs::srv::SetSpeedSliderFraction>(
+        "~/set_speed_slider",
+        std::bind(&GPIOController::setSpeedSlider, this, std::placeholders::_1, std::placeholders::_2));
+  } catch (...) {
+    return LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
   return LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 ur_controllers::GPIOController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
+  try {
+    // reset publisher
+    io_pub_.reset();
+    tool_data_pub_.reset();
+    robot_mode_pub_.reset();
+    safety_mode_pub_.reset();
+    set_io_srv_.reset();
+    set_speed_slider_srv_.reset();
+  } catch (...) {
+    return LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
   return LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 

ur_robot_driver/test/integration_test.py

@@ -115,24 +115,20 @@ def test_switch_on(self):
         empty_req = Trigger.Request()
         get_robot_mode_req = GetRobotMode.Request()
 
-        self.power_on_client.call_async(empty_req)
+        self.call_service(self.power_on_client, empty_req)
         end_time = time.time() + 10
         mode = RobotMode.DISCONNECTED
         while mode not in (RobotMode.IDLE, RobotMode.RUNNING) and time.time() < end_time:
-            future = self.get_robot_mode_client.call_async(get_robot_mode_req)
-            while future.done() is False:
-                rclpy.spin_once(self.node, timeout_sec=0.1)
-            mode = future.result().robot_mode.mode
+            result = self.call_service(self.get_robot_mode_client, get_robot_mode_req)
+            mode = result.robot_mode.mode
 
         self.assertIn(mode, (RobotMode.IDLE, RobotMode.RUNNING))
 
-        self.brake_release_client.call_async(empty_req)
+        self.call_service(self.brake_release_client, empty_req)
         end_time = time.time() + 10
         while mode != RobotMode.RUNNING and time.time() < end_time:
-            future = self.get_robot_mode_client.call_async(get_robot_mode_req)
-            while future.done() is False:
-                rclpy.spin_once(self.node, timeout_sec=0.1)
-            mode = future.result().robot_mode.mode
+            result = self.call_service(self.get_robot_mode_client, get_robot_mode_req)
+            mode = result.robot_mode.mode
 
         self.assertEqual(mode, RobotMode.RUNNING)
 
@@ -152,7 +148,7 @@ def test_set_io(self):
         set_io_req.pin = pin
         set_io_req.state = 1.0
 
-        self.set_io_client.call_async(set_io_req)
+        self.call_service(self.set_io_client, set_io_req)
         pin_state = False
 
         end_time = time.time() + 5
@@ -164,7 +160,7 @@ def test_set_io(self):
         self.assertEqual(pin_state, 1)
 
         set_io_req.state = 0.0
-        self.set_io_client.call_async(set_io_req)
+        self.call_service(self.set_io_client, set_io_req)
 
         end_time = time.time() + 5
         while pin_state and time.time() < end_time:
@@ -178,3 +174,11 @@ def test_set_io(self):
 
     def io_msg_cb(self, msg):
         self.io_msg = msg
+
+    def call_service(self, client, request):
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self.node, future)
+        if future.result() is not None:
+            return future.result()
+        else:
+            raise Exception(f"Exception while calling service: {future.exception()}")