Update message_handler.py

ros2/src/robobin/robobin/helpers/message_handler.py

@@ -14,7 +14,7 @@ class MessageHandler:
             "SETMODE": self.handle_set_mode,
             "STOP": self.handle_stop,
             "CALLME": self.handle_call_me,
-            "BPM": self.handle_call_bpm,
+            "CALLBPM": self.handle_call_bpm,
             "REQMAP": self.handle_request_map,
             "LOCATION": self.handle_request_location,
             "CALLMELIDAR": self.handle_call_me_lidar
@@ -32,6 +32,9 @@ class MessageHandler:
 
     def handle_request_location(self, client_socket, message):
         """Handles the LOC command."""
+        if self.testing:
+            print("Received LOC command")
+            return "nav_command", "LOC"
         response = f"location is {self.api_node.connection_manager.location}".encode()
         if self.testing:
             print(response.decode())
@@ -56,7 +59,9 @@ class MessageHandler:
         print(message)
         location = message.strip() # Remove parentheses
         location = f"{location}"  # Add parentheses back for proper formatting
-        
+        if self.testing:
+            print(f"Received CALLME command from {ip} with location {location}")
+            return None
         for existing_socket, existing_location in self.api_node.called_locations:
                 if existing_socket == client_socket:
                     client_socket.sendall(b"User already requested location")
@@ -78,7 +83,7 @@ class MessageHandler:
             print(response.decode())
         else:
             client_socket.sendall(response)
-        self.api_node.called_locations = [] # Clear the queue
+            self.api_node.called_locations = [] # Clear the queue
         return "cmd_vel", (0.0, 0.0)
     
     def handle_ping(self, client_socket, _):
@@ -95,11 +100,13 @@ class MessageHandler:
         response = f"Setting mode to {message}".encode()
         if self.testing:
             print(response.decode())
+            return "nav_command", "SETMODE " + message
         else:
             client_socket.sendall(response)
-        self.api_node.mode = message
-        self.api_node.called_locations = [] # Clear the queue
-        return "nav_command", "SETMODE " + message
+        
+            self.api_node.mode = message
+            self.api_node.called_locations = [] # Clear the queue
+            return "nav_command", "SETMODE " + message
     def handle_time_request(self, client_socket, _):
         """Sends the current server time."""
         response = time.ctime().encode()
@@ -150,7 +157,7 @@ class MessageHandler:
             """Handles the REQMAP command."""
             # Relative path to the JSON file
             working_dir = os.getcwd()
-            graph_file_path =  os.path.abspath(os.path.join(working_dir,"src", "robobin", "robobin", "graphs", "graph.json"))
+            graph_file_path =  os.path.abspath(os.path.join(working_dir,"src", "robobin", "robobin", "graphs", "anchors.json"))
 
             try:
                 # Load the graph JSON file
@@ -213,6 +220,10 @@ if __name__ == "__main__":
     assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.25))
 
     assert message_handler.handle_message(None, "STOP") == ("cmd_vel", (0.0, 0.0))
-    assert message_handler.handle_message(None, "CALLME (212.9638, 283.98108)") == ("nav_command", "(212.9638, 283.98108)")
+    assert message_handler.handle_message(None, "CALLME (212.9638, 283.98108)") == None
     assert message_handler.handle_message(None, "UNKNOWN") is None
-    assert message_handler.handle_message(None, "REQMAP") is None
+    assert message_handler.handle_message(None, "REQMAP") is None
\ No newline at end of file
+    assert message_handler.handle_message(None, "SETMODE MANUAL") == ("nav_command", "SETMODE MANUAL")
+    assert message_handler.handle_message(None, "CALLBPM")== ("nav_command", "BPM")
+    assert message_handler.handle_message(None, "LOCATION") is ("nav_command", "LOC")
+    assert message_handler.handle_message(None, "CALLMELIDAR 1") == ("nav_command_lidar", 1)
\ No newline at end of file