Converted to a lifecycle node

face_following/face_following/face_following_node.py

@@ -1,5 +1,11 @@
+from typing import Optional
+
 import rclpy
-from rclpy.node import Node
+from rclpy.lifecycle import Node
+from rclpy.lifecycle import Publisher
+from rclpy.lifecycle import State
+from rclpy.lifecycle import TransitionCallbackReturn
+from rclpy.timer import Timer
 import std_msgs
 import std_msgs.msg
 import vision_msgs.msg
@@ -29,23 +35,17 @@ def __init__(self):
         self.fov_h = self.get_parameter("fov_h").get_parameter_value().integer_value
 
         # Subscriptions
-        self.subscription = self.create_subscription(
+        self.subscription_bounding_box = self.create_subscription(
             vision_msgs.msg.BoundingBox2D,
             "/face_bounding_box",
-            self.bounding_box_callback,
+            self.callback_bounding_box,
             1,
         )
 
-        # Publishers
-        self.publisher_servo_pan = self.create_publisher(
-            std_msgs.msg.Float64, "/servo_pan/set_error", 1
-        )
-        self.publisher_servo_tilt = self.create_publisher(
-            std_msgs.msg.Float64, "/servo_tilt/set_error", 1
-        )
-
-        # Timers
-        self.timer = self.create_timer(self.timer_period, self.timer_callback)
+        # Lifecycle Node timers and publishers
+        self.timer: Optional[Timer] = None
+        self.publisher_servo_pan: Optional[Publisher] = None
+        self.publisher_servo_tilt: Optional[Publisher] = None
 
         # Node variables
         self.detection_time_stamp = self.get_clock().now()
@@ -58,14 +58,21 @@ def __init__(self):
         self.target_x = self.center_x
         self.target_y = self.center_y
 
-    def bounding_box_callback(self, msg):
+    ##################### Callbacks #####################
+
+    def callback_bounding_box(self, msg):
 
         self.target_x = msg.center.position.x
         self.target_y = msg.center.position.y
 
         self.detection_time_stamp = self.get_clock().now()
 
-    def timer_callback(self):
+    def callback_timer(self):
+
+        if self.publisher_servo_pan is not None:
+            return
+        if self.publisher_servo_tilt is not None:
+            return
 
         time_current = self.get_clock().now()
         time_diff = (
@@ -92,6 +99,50 @@ def timer_callback(self):
         self.publisher_servo_pan.publish(std_msgs.msg.Float64(data=float(error_pan)))
         self.publisher_servo_tilt.publish(std_msgs.msg.Float64(data=float(error_tilt)))
 
+        ##################### Lifecyle Node Functions #####################
+
+    def on_configure(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info("on_configure() is called.")
+
+        # Publishers
+        self.publisher_servo_pan = self.create_publisher(
+            std_msgs.msg.Float64, "/servo_pan/set_error", 1
+        )
+        self.publisher_servo_tilt = self.create_publisher(
+            std_msgs.msg.Float64, "/servo_tilt/set_error", 1
+        )
+
+        # Timers
+        self.timer = self.create_timer(self.timer_period, self.callback_timer)
+
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_activate(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info("on_activate() is called.")
+        return super().on_activate(state)
+
+    def on_deactivate(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info("on_deactivate() is called.")
+        return super().on_deactivate(state)
+
+    def on_cleanup(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info("on_cleanup() is called.")
+
+        self.destroy_timer(self.timer)
+        self.destroy_publisher(self.publisher_servo_pan)
+        self.destroy_publisher(self.publisher_servo_tilt)
+
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_shutdown(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info("on_shutdown() is called.")
+
+        self.destroy_timer(self.timer)
+        self.destroy_publisher(self.publisher_servo_pan)
+        self.destroy_publisher(self.publisher_servo_tilt)
+
+        return TransitionCallbackReturn.SUCCESS
+
 
 def main(args=None):
     rclpy.init(args=args)

face_following/package.xml

@@ -9,6 +9,9 @@
 
   <exec_depend>servo_control</exec_depend>
 
+  <depend>lifecycle_msgs</depend>
+  <depend>lifecycle</depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>