@@ -35,16 +35,16 @@ def ping(host):
3535 return False
3636
3737
38- class IntegrateDatabase (Node ):
38+ class Mocha (Node ):
3939 def __init__ (self ):
40- super ().__init__ ("integrate_database " )
40+ super ().__init__ ("mocha_setver " )
4141
4242 # Handle shutdown signal
4343 self .shutdownTriggered = threading .Event ()
4444 self .shutdownTriggered .clear ()
4545
4646 def signal_handler (sig , frame ):
47- self .logger .warning (f"{ self .this_robot } - Integrate - Got SIGINT. Triggering shutdown." )
47+ self .logger .warning (f"{ self .this_robot } - MOCHA Server - Got SIGINT. Triggering shutdown." )
4848 self .shutdown ("Killed by user" )
4949 signal .signal (signal .SIGINT , signal_handler )
5050
@@ -63,13 +63,13 @@ def signal_handler(sig, frame):
6363 self .rssi_threshold = self .get_parameter ("rssi_threshold" ).get_parameter_value ().integer_value
6464
6565 if len (self .this_robot ) == 0 :
66- self .logger .error (f"{ self .this_robot } - Integrate - Empty robot name" )
66+ self .logger .error (f"{ self .this_robot } - MOCHA Server - Empty robot name" )
6767 raise ValueError ("Empty robot name" )
6868
69- self .logger .info (f"{ self .this_robot } - Integrate - " +
69+ self .logger .info (f"{ self .this_robot } - MOCHA Server - " +
7070 f"RSSI threshold: { self .rssi_threshold } " )
7171 self .client_timeout = self .get_parameter ("client_timeout" ).get_parameter_value ().double_value
72- self .logger .info (f"{ self .this_robot } - Integrate - " +
72+ self .logger .info (f"{ self .this_robot } - MOCHA Server - " +
7373 f"Client timeout: { self .client_timeout } " )
7474
7575 # Load and check robot configs
@@ -78,10 +78,10 @@ def signal_handler(sig, frame):
7878 with open (self .robot_configs_file , "r" ) as f :
7979 self .robot_configs = yaml .load (f , Loader = yaml .FullLoader )
8080 except Exception as e :
81- self .logger .error (f"{ self .this_robot } - Integrate - robot_configs file" )
81+ self .logger .error (f"{ self .this_robot } - MOCHA Server - robot_configs file" )
8282 raise e
8383 if self .this_robot not in self .robot_configs .keys ():
84- self .logger .error (f"{ self .this_robot } - Integrate - robot_configs file" )
84+ self .logger .error (f"{ self .this_robot } - MOCHA Server - robot_configs file" )
8585 raise ValueError ("Robot not in config file" )
8686
8787 # Load and check radio configs
@@ -90,11 +90,11 @@ def signal_handler(sig, frame):
9090 with open (self .radio_configs_file , "r" ) as f :
9191 self .radio_configs = yaml .load (f , Loader = yaml .FullLoader )
9292 except Exception as e :
93- self .logger .error (f"{ self .this_robot } - Integrate - radio_configs file" )
93+ self .logger .error (f"{ self .this_robot } - MOCHA Server - radio_configs file" )
9494 raise e
9595 self .radio = self .robot_configs [self .this_robot ]["using-radio" ]
9696 if self .radio not in self .radio_configs .keys ():
97- self .logger .error (f"{ self .this_robot } - Integrate - radio_configs file" )
97+ self .logger .error (f"{ self .this_robot } - MOCHA Server - radio_configs file" )
9898 raise ValueError ("Radio {self.radio} not in config file" )
9999
100100 # Load and check topic configs
@@ -103,17 +103,17 @@ def signal_handler(sig, frame):
103103 with open (self .topic_configs_file , "r" ) as f :
104104 self .topic_configs = yaml .load (f , Loader = yaml .FullLoader )
105105 except Exception as e :
106- self .logger .error (f"{ self .this_robot } - Integrate - topics_configs file" )
106+ self .logger .error (f"{ self .this_robot } - MOCHA Server - topics_configs file" )
107107 raise e
108108 self_type = self .robot_configs [self .this_robot ]["node-type" ]
109109 if self_type not in self .topic_configs .keys ():
110- self .logger .error (f"{ self .this_robot } - Integrate - topics_configs file" )
110+ self .logger .error (f"{ self .this_robot } - MOCHA Server - topics_configs file" )
111111 raise ValueError ("Node type not in config file" )
112112
113113 # Check that we can ping the radios
114114 ip = self .robot_configs [self .this_robot ]["IP-address" ]
115115 if not ping (ip ):
116- self .logger .error (f"{ self .this_robot } - Integrate - " +
116+ self .logger .error (f"{ self .this_robot } - MOCHA Server - " +
117117 f"Cannot ping self { ip } . Is the radio on?" )
118118 raise ValueError ("Cannot ping self" )
119119
@@ -124,7 +124,7 @@ def signal_handler(sig, frame):
124124
125125 self .num_robot_in_comm = 0
126126
127- self .logger .info (f"{ self .this_robot } - Integrate - " +
127+ self .logger .info (f"{ self .this_robot } - MOCHA Server - " +
128128 "Created all communication channels!" )
129129
130130 # Start comm channels with other robots
@@ -134,7 +134,7 @@ def signal_handler(sig, frame):
134134 for other_robot in self .other_robots :
135135 if other_robot not in self .robot_configs [self .this_robot ]["clients" ]:
136136 self .logger .warning (
137- f"{ self .this_robot } - Integrate - " +
137+ f"{ self .this_robot } - MOCHA Server - " +
138138 f"Skipping channel { self .this_robot } ->{ other_robot } " +
139139 "as it is not in the graph of this robot"
140140 )
@@ -167,16 +167,16 @@ def shutdown(self, reason):
167167 self .shutdownTriggered .set ()
168168
169169 assert isinstance (reason , str )
170- self .logger .error (f"{ self .this_robot } - Integrate - " + reason )
170+ self .logger .error (f"{ self .this_robot } - MOCHA Server - " + reason )
171171 # Shutting down communication channels
172172 if hasattr (self , 'all_channels' ) and len (self .all_channels ) != 0 :
173173 for channel in self .all_channels :
174174 channel .stop ()
175- self .logger .warning (f"{ self .this_robot } - Integrate - " + "Killed Channels" )
175+ self .logger .warning (f"{ self .this_robot } - MOCHA Server - " + "Killed Channels" )
176176 # Wait for all the channels to be gone. This needs to be slightly
177177 # larger than RECV_TIMEOUT
178178 time .sleep (3.5 )
179- self .logger .warning (f"{ self .this_robot } - Integrate - " + "Shutdown complete" )
179+ self .logger .warning (f"{ self .this_robot } - MOCHA Server - " + "Shutdown complete" )
180180
181181 def rssi_cb (self , data , comm_node ):
182182 rssi = data .data
@@ -195,30 +195,30 @@ def main(args=None):
195195
196196 # Start the node
197197 try :
198- integrate_db = IntegrateDatabase ()
198+ mocha = Mocha ()
199199 except Exception as e :
200200 print (f"Node initialization failed: { e } " )
201201 rclpy .shutdown ()
202202 return
203203
204204 # Load mtexecutor
205205 mtexecutor = MultiThreadedExecutor (num_threads = 4 )
206- mtexecutor .add_node (integrate_db )
206+ mtexecutor .add_node (mocha )
207207
208208 # Use context manager for clean shutdown
209209 try :
210210 # Spin with periodic checking for shutdown
211- while rclpy .ok () and not integrate_db .shutdownTriggered .is_set ():
211+ while rclpy .ok () and not mocha .shutdownTriggered .is_set ():
212212 mtexecutor .spin_once (timeout_sec = 0.1 )
213213 except KeyboardInterrupt :
214214 print ("Keyboard Interrupt" )
215- integrate_db .shutdown ("KeyboardInterrupt" )
215+ mocha .shutdown ("KeyboardInterrupt" )
216216 except Exception as e :
217217 print (f"Exception: { e } " )
218- integrate_db .shutdown (f"Exception: { e } " )
218+ mocha .shutdown (f"Exception: { e } " )
219219 finally :
220220 # Clean up node and ROS2 from main thread (safe)
221- integrate_db .destroy_node ()
221+ mocha .destroy_node ()
222222 rclpy .shutdown ()
223223
224224if __name__ == "__main__" :
0 commit comments