@@ -35,12 +35,20 @@ void DeepNodeBase::declare_parameters()
3535{
3636 declare_parameter (" Backend.plugin" , " " );
3737 declare_parameter (" model_path" , " " );
38+
39+ // Bond parameters
40+ declare_parameter (" Bond.enable" , false );
41+ declare_parameter (" Bond.bond_timeout" , 4.0 );
42+ declare_parameter (" Bond.bond_heartbeat_period" , 0.1 );
3843}
3944
4045CallbackReturn DeepNodeBase::on_configure (const rclcpp_lifecycle::State & state)
4146{
4247 RCLCPP_INFO (get_logger (), " Configuring DeepNodeBase" );
4348
49+ // Setup bond if enabled
50+ setup_bond ();
51+
4452 std::string backend_plugin = get_parameter (" Backend.plugin" ).as_string ();
4553 if (!backend_plugin.empty ()) {
4654 if (!load_plugin (backend_plugin)) {
@@ -63,6 +71,13 @@ CallbackReturn DeepNodeBase::on_configure(const rclcpp_lifecycle::State & state)
6371CallbackReturn DeepNodeBase::on_activate (const rclcpp_lifecycle::State & state)
6472{
6573 RCLCPP_INFO (get_logger (), " Activating DeepNodeBase" );
74+
75+ // Start bond if enabled
76+ if (bond_enabled_ && bond_) {
77+ bond_->start ();
78+ RCLCPP_INFO (get_logger (), " Bond started" );
79+ }
80+
6681 return on_activate_impl (state);
6782}
6883
@@ -75,6 +90,13 @@ CallbackReturn DeepNodeBase::on_deactivate(const rclcpp_lifecycle::State & state
7590CallbackReturn DeepNodeBase::on_cleanup (const rclcpp_lifecycle::State & state)
7691{
7792 RCLCPP_INFO (get_logger (), " Cleaning up DeepNodeBase" );
93+
94+ // Stop bond if active
95+ if (bond_) {
96+ bond_.reset ();
97+ RCLCPP_INFO (get_logger (), " Bond stopped and cleaned up" );
98+ }
99+
78100 unload_model ();
79101 plugin_.reset ();
80102 return on_cleanup_impl (state);
@@ -83,6 +105,13 @@ CallbackReturn DeepNodeBase::on_cleanup(const rclcpp_lifecycle::State & state)
83105CallbackReturn DeepNodeBase::on_shutdown (const rclcpp_lifecycle::State & state)
84106{
85107 RCLCPP_INFO (get_logger (), " Shutting down DeepNodeBase" );
108+
109+ // Stop bond if active
110+ if (bond_) {
111+ bond_.reset ();
112+ RCLCPP_INFO (get_logger (), " Bond stopped and cleaned up" );
113+ }
114+
86115 unload_model ();
87116 plugin_.reset ();
88117 return on_shutdown_impl (state);
@@ -187,4 +216,28 @@ pluginlib::UniquePtr<DeepBackendPlugin> DeepNodeBase::load_plugin_library(const
187216 return plugin_loader_->createUniqueInstance (plugin_name);
188217}
189218
219+ void DeepNodeBase::setup_bond ()
220+ {
221+ bond_enabled_ = get_parameter (" Bond.enable" ).as_bool ();
222+ bond_timeout_ = get_parameter (" Bond.bond_timeout" ).as_double ();
223+ bond_heartbeat_period_ = get_parameter (" Bond.bond_heartbeat_period" ).as_double ();
224+
225+ if (bond_enabled_) {
226+ RCLCPP_INFO (
227+ get_logger (), " Setting up bond with timeout: %.1fs, heartbeat: %.1fs" , bond_timeout_, bond_heartbeat_period_);
228+
229+ // Create bond with standard topic name and node name as bond ID
230+ std::string bond_topic = " /bond" ;
231+ bond_ = std::make_unique<bond::Bond>(bond_topic, get_name (), shared_from_this ());
232+
233+ // Configure bond parameters
234+ bond_->setHeartbeatPeriod (bond_heartbeat_period_);
235+ bond_->setHeartbeatTimeout (bond_timeout_);
236+
237+ RCLCPP_INFO (get_logger (), " Bond configured on topic: %s" , bond_topic.c_str ());
238+ } else {
239+ RCLCPP_INFO (get_logger (), " Bond disabled" );
240+ }
241+ }
242+
190243} // namespace deep_ros
0 commit comments