@@ -55,10 +55,8 @@ def __init__(self) -> None:
55
55
self .declare_parameter ("image_reliability" ,
56
56
QoSReliabilityPolicy .BEST_EFFORT )
57
57
58
- self .get_logger ().info ("Debug node created" )
59
-
60
58
def on_configure (self , state : LifecycleState ) -> TransitionCallbackReturn :
61
- self .get_logger ().info (f"Configuring { self .get_name ()} " )
59
+ self .get_logger ().info (f"[ { self .get_name ()} ] Configuring... " )
62
60
63
61
self .image_qos_profile = QoSProfile (
64
62
reliability = self .get_parameter (
@@ -75,10 +73,13 @@ def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn:
75
73
self ._kp_markers_pub = self .create_publisher (
76
74
MarkerArray , "dgb_kp_markers" , 10 )
77
75
76
+ super ().on_configure (state )
77
+ self .get_logger ().info (f"[{ self .get_name ()} ] Configured" )
78
+
78
79
return TransitionCallbackReturn .SUCCESS
79
80
80
81
def on_activate (self , state : LifecycleState ) -> TransitionCallbackReturn :
81
- self .get_logger ().info (f"Activating { self .get_name ()} " )
82
+ self .get_logger ().info (f"[ { self .get_name ()} ] Activating... " )
82
83
83
84
# subs
84
85
self .image_sub = message_filters .Subscriber (
@@ -90,25 +91,40 @@ def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn:
90
91
(self .image_sub , self .detections_sub ), 10 , 0.5 )
91
92
self ._synchronizer .registerCallback (self .detections_cb )
92
93
94
+ super ().on_activate (state )
95
+ self .get_logger ().info (f"[{ self .get_name ()} ] Activated" )
96
+
93
97
return TransitionCallbackReturn .SUCCESS
94
98
95
99
def on_deactivate (self , state : LifecycleState ) -> TransitionCallbackReturn :
96
- self .get_logger ().info (f"Deactivating { self .get_name ()} " )
100
+ self .get_logger ().info (f"[ { self .get_name ()} ] Deactivating... " )
97
101
98
102
self .destroy_subscription (self .image_sub .sub )
99
103
self .destroy_subscription (self .detections_sub .sub )
100
104
101
105
del self ._synchronizer
102
106
107
+ super ().on_deactivate (state )
108
+ self .get_logger ().info (f"[{ self .get_name ()} ] Deactivated" )
109
+
103
110
return TransitionCallbackReturn .SUCCESS
104
111
105
112
def on_cleanup (self , state : LifecycleState ) -> TransitionCallbackReturn :
106
- self .get_logger ().info (f"Cleaning up { self .get_name ()} " )
113
+ self .get_logger ().info (f"[ { self .get_name ()} ] Cleaning up... " )
107
114
108
115
self .destroy_publisher (self ._dbg_pub )
109
116
self .destroy_publisher (self ._bb_markers_pub )
110
117
self .destroy_publisher (self ._kp_markers_pub )
111
118
119
+ super ().on_cleanup (state )
120
+ self .get_logger ().info (f"[{ self .get_name ()} ] Cleaned up" )
121
+
122
+ return TransitionCallbackReturn .SUCCESS
123
+
124
+ def on_shutdown (self , state : LifecycleState ) -> TransitionCallbackReturn :
125
+ self .get_logger ().info (f"[{ self .get_name ()} ] Shutting down..." )
126
+ super ().on_cleanup (state )
127
+ self .get_logger ().info (f"[{ self .get_name ()} ] Shutted down" )
112
128
return TransitionCallbackReturn .SUCCESS
113
129
114
130
def draw_box (self , cv_image : np .ndarray , detection : Detection , color : Tuple [int ]) -> np .ndarray :
0 commit comments