import asyncio import datetime import json import time import random import paho.mqtt.client as mqtt from typing import Dict, List, Optional, Any, Tuple import config import mqtt_utils import utils from protocol.vda5050_common import AgvPosition from protocol.vda_2_0_0.vda5050_2_0_0_connection import Connection, ConnectionState from protocol.vda_2_0_0.vda5050_2_0_0_state import State, NodeState, EdgeState, ActionState, BatteryState, SafetyState, OperatingMode, ActionStatus, EStop from protocol.vda_2_0_0.vda5050_2_0_0_visualization import Visualization from protocol.vda_2_0_0.vda5050_2_0_0_action import Action, ActionParameter, ActionParameterValue, BlockingType from protocol.vda_2_0_0.vda5050_2_0_0_order import Order from protocol.vda_2_0_0.vda5050_2_0_0_instant_actions import InstantActions class VehicleSimulator: """Simulates a VDA5050 vehicle""" def __init__(self, config_data: config.Config): self.config = config_data # Generate base MQTT topic base_topic = mqtt_utils.generate_vda_mqtt_base_topic( self.config.mqtt_broker.vda_interface, self.config.vehicle.vda_version, self.config.vehicle.manufacturer, self.config.vehicle.serial_number ) # Connection self.connection_topic = f"{base_topic}/connection" self.connection = Connection( header_id=0, timestamp=utils.get_timestamp(), version=self.config.vehicle.vda_full_version, manufacturer=self.config.vehicle.manufacturer, serial_number=self.config.vehicle.serial_number, connection_state=ConnectionState.CONNECTION_BROKEN ) # State self.state_topic = f"{base_topic}/state" random_x = random.random() * 5.0 - 2.5 random_y = random.random() * 5.0 - 2.5 agv_position = AgvPosition( x=random_x, y=random_y, position_initialized=True, theta=0.0, map_id=self.config.settings.map_id, deviation_range=None, map_description=None, localization_score=None ) self.state = State( header_id=0, timestamp=utils.get_timestamp(), version=self.config.vehicle.vda_full_version, manufacturer=self.config.vehicle.manufacturer, serial_number=self.config.vehicle.serial_number, driving=False, distance_since_last_node=None, operating_mode=OperatingMode.AUTOMATIC, node_states=[], edge_states=[], last_node_id="", order_id="", order_update_id=0, last_node_sequence_id=0, action_states=[], information=[], loads=[], errors=[], battery_state=BatteryState( battery_charge=100.0, battery_voltage=None, battery_health=None, charging=False, reach=None ), safety_state=SafetyState( e_stop=EStop.NONE, field_violation=False ), paused=None, new_base_request=None, agv_position=agv_position, velocity=None, zone_set_id=None ) # Visualization self.visualization_topic = f"{base_topic}/visualization" self.visualization = Visualization( header_id=0, timestamp=utils.get_timestamp(), version=self.config.vehicle.vda_full_version, manufacturer=self.config.vehicle.manufacturer, serial_number=self.config.vehicle.serial_number, agv_position=agv_position, velocity=None ) # Order and Instant Actions self.order = None self.instant_actions = None self.action_start_time = None def run_action(self, action: Action) -> None: """Execute an action""" action_state_index = None for i, a_state in enumerate(self.state.action_states): if a_state.action_id == action.action_id: # Ensure we only try to run actions that are WAITING if a_state.action_status == ActionStatus.WAITING: action_state_index = i else: # Action already running, finished, etc. - don't restart return False break if action_state_index is not None: action_state = self.state.action_states[action_state_index] action_state.action_status = ActionStatus.RUNNING print(f"SIM ({self.config.vehicle.serial_number}): Running action ID {action.action_id}, Type: {action.action_type}") if action.action_type == "initPosition": # Find parameters x_param = None y_param = None theta_param = None map_id_param = None for param in action.action_parameters: if param.key == "x": x_param = param.value elif param.key == "y": y_param = param.value elif param.key == "theta": theta_param = param.value elif param.key == "mapId": map_id_param = param.value # Extract values x_float = float(x_param.value) if x_param else 0.0 y_float = float(y_param.value) if y_param else 0.0 theta_float = float(theta_param.value) if theta_param else 0.0 map_id_string = str(map_id_param.value) if map_id_param else "" # Update position self.state.agv_position = AgvPosition( x=x_float, y=y_float, position_initialized=True, theta=theta_float, map_id=map_id_string, deviation_range=None, map_description=None, localization_score=None ) self.visualization.agv_position = self.state.agv_position # initPosition is instantaneous in this sim, mark finished immediately action_state.action_status = ActionStatus.FINISHED print(f"SIM ({self.config.vehicle.serial_number}): Finished action ID {action.action_id}, Type: {action.action_type}") # No action timer needed for instant actions like initPosition return True # Action handled elif action.action_type in ["pickUp", "pickup"]: # Simulate starting the pick-up process print(f"SIM ({self.config.vehicle.serial_number}): Starting pickUp action ID {action.action_id}") self.action_start_time = datetime.datetime.utcnow() return True # Action started elif action.action_type in ["dropOff", "dropoff"]: # Simulate starting the drop-off process print(f"SIM ({self.config.vehicle.serial_number}): Starting dropOff action ID {action.action_id}") self.action_start_time = datetime.datetime.utcnow() return True # Action started elif action.action_type == "charge": # Simulate starting the charging process print(f"SIM ({self.config.vehicle.serial_number}): Starting charging action ID {action.action_id}") self.state.battery_state.battery_charge = 100.0 # Replenish battery to 100% self.action_start_time = datetime.datetime.utcnow() return True # Action started else: # Handle unknown action types (optional: mark as failed or just finish) print(f"SIM ({self.config.vehicle.serial_number}): Unknown action type '{action.action_type}'. Marking as finished.") action_state.action_status = ActionStatus.FINISHED return True # Action handled (by finishing) # Mark action as finished # self.state.action_states[action_state_index].action_status = ActionStatus.FINISHED return False # Action not found or not in WAITING state async def publish_connection(self, mqtt_client: mqtt.Client) -> None: """Publish connection state""" # First publish ConnectionBroken state json_connection_broken = json.dumps(self.connection.to_dict()) await mqtt_utils.mqtt_publish(mqtt_client, self.connection_topic, json_connection_broken) # Wait for 1 second await asyncio.sleep(1) # Update to Online state self.connection.header_id += 1 self.connection.timestamp = utils.get_timestamp() self.connection.connection_state = ConnectionState.ONLINE json_connection_online = json.dumps(self.connection.to_dict()) await mqtt_utils.mqtt_publish(mqtt_client, self.connection_topic, json_connection_online) async def publish_visualization(self, mqtt_client: mqtt.Client) -> None: """Publish visualization data""" self.visualization.header_id += 1 self.visualization.timestamp = utils.get_timestamp() json_visualization = json.dumps(self.visualization.to_dict()) await mqtt_utils.mqtt_publish(mqtt_client, self.visualization_topic, json_visualization) async def publish_state(self, mqtt_client: mqtt.Client) -> None: """Publish state data""" self.state.header_id += 1 self.state.timestamp = utils.get_timestamp() json_state = json.dumps(self.state.to_dict()) await mqtt_utils.mqtt_publish(mqtt_client, self.state_topic, json_state) def instant_actions_accept_procedure(self, instant_action_request: InstantActions) -> None: """Process incoming instant actions""" # TODO: Add validation self.instant_actions = instant_action_request # Add instant actions to action states for instant_action in self.instant_actions.instant_actions: action_state = ActionState( action_id=instant_action.action_id, action_status=ActionStatus.WAITING, action_type=instant_action.action_type, action_description=None, result_description=None ) self.state.action_states.append(action_state) def order_accept_procedure(self, order_request: Order) -> None: """Process incoming order request""" if order_request.order_id != self.state.order_id: # Empty string (""), if no previous orderId is available if self.state.order_id == "": self.order_accept(order_request) return # Check action states if len(self.state.node_states) == 0 and len(self.state.edge_states) == 0: # Delete action states self.state.action_states = [] self.order_accept(order_request) return else: self.order_reject("There is order_state or edge_state in state") return else: if order_request.order_update_id > self.state.order_update_id: if len(self.state.node_states) > 0 and len(self.state.edge_states) == 0: # Delete action states self.state.action_states = [] self.order_accept(order_request) return else: self.order_reject("There is order_state or edge_state in state1") return else: self.order_reject("Order update id is lower") return def order_accept(self, order_request: Order) -> None: """Accept an order""" self.order = order_request # Find the first node in the order based on sequence ID. # VDA5050 sequence IDs usually start at 1. Find the node with the lowest sequence ID. first_node = None if self.order.nodes: # Sort nodes by sequence ID just in case they aren't ordered in the request sorted_nodes = sorted(self.order.nodes, key=lambda n: n.sequence_id) first_node = sorted_nodes[0] if first_node: # Set initial last_node and sequence to the *first* node of the new order. # This makes the state ready to find the *next* node (the second one) # or execute actions on the first node. self.state.last_node_id = first_node.node_id self.state.last_node_sequence_id = first_node.sequence_id print(f"SIMULATOR ({self.config.vehicle.serial_number}): Initializing state to first node: ID={self.state.last_node_id}, Seq={self.state.last_node_sequence_id}") else: # Handle case where order has no nodes (edge case) self.state.last_node_id = "" self.state.last_node_sequence_id = 0 print(f"SIMULATOR ({self.config.vehicle.serial_number}): Warning - Accepted order has no nodes.") # Set orderId and orderUpdateId *after* determining the starting node info self.state.order_id = self.order.order_id self.state.order_update_id = self.order.order_update_id # Delete old states self.state.action_states = [] self.state.node_states = [] self.state.edge_states = [] # Set nodeStates, edgeStates, and actionStates for node in self.order.nodes: node_state = NodeState( node_id=node.node_id, sequence_id=node.sequence_id, released=node.released, node_description=node.node_description, node_position=node.node_position ) self.state.node_states.append(node_state) for action in node.actions: action_state = ActionState( action_id=action.action_id, action_type=action.action_type, action_description=action.action_description, action_status=ActionStatus.WAITING, result_description=None ) self.state.action_states.append(action_state) for edge in self.order.edges: edge_state = EdgeState( edge_id=edge.edge_id, sequence_id=edge.sequence_id, released=edge.released, start_node_id=edge.start_node_id, end_node_id=edge.end_node_id, edge_description=edge.edge_description, trajectory=None ) self.state.edge_states.append(edge_state) for action in edge.actions: action_state = ActionState( action_id=action.action_id, action_type=action.action_type, action_description=action.action_description, action_status=ActionStatus.WAITING, result_description=None ) self.state.action_states.append(action_state) def order_reject(self, reason: str) -> None: """Reject an order""" print(f"Order reject: {reason}") def state_iterate(self) -> None: """Update state based on simulation logic""" # --- Battery consumption --- if self.state.battery_state.battery_charge > 0: if self.state.driving: # Consume 0.01% per tick when driving (approx 0.2% per second) self.state.battery_state.battery_charge = max(0.0, round(self.state.battery_state.battery_charge - 0.01, 2)) else: # Consume 0.001% per tick when idle (approx 0.02% per second) self.state.battery_state.battery_charge = max(0.0, round(self.state.battery_state.battery_charge - 0.001, 2)) # If battery is empty, force driving to False, print message, and halt simulation iteration if self.state.battery_state.battery_charge <= 0: self.state.driving = False print(f"SIM ({self.config.vehicle.serial_number}): Battery empty! Simulation stopped.") return # Default driving to False at start of tick logic (will be set to True if moving) self.state.driving = False # --- >> CHANGE ACTION HANDLING LOGIC << --- # 1. Check if currently executing a timed action is_action_running = False running_action_id = None if self.action_start_time is not None: current_time = datetime.datetime.utcnow() action_end_time = self.action_start_time + datetime.timedelta(seconds=self.config.settings.action_time) # Find the action state that is RUNNING running_action_state = None for a_state in self.state.action_states: if a_state.action_status == ActionStatus.RUNNING: # Assume only one timed action runs at a time in this simple sim running_action_state = a_state break if running_action_state: if current_time < action_end_time: # Action is still running, don't move is_action_running = True running_action_id = running_action_state.action_id # print(f"DEBUG ({self.config.vehicle.serial_number}): Action {running_action_id} still running...") # return # Optional: maybe still allow instant actions? For now, block all. else: # Action finished print(f"SIM ({self.config.vehicle.serial_number}): Finished timed action ID {running_action_state.action_id}, Type: {running_action_state.action_type}") running_action_state.action_status = ActionStatus.FINISHED self.action_start_time = None # Clear the timer # Continue iteration to potentially start moving again # If a timed action is still running, potentially block further processing if is_action_running: # Here, decide if instant actions or other logic can proceed even if a timed action is running # For simplicity, we will block movement if a timed action is running # We might still allow processing newly received instant actions below if needed. # print(f"DEBUG ({self.config.vehicle.serial_number}): Timed action {running_action_id} is running, blocking movement.") pass # Allow instant action check below, but movement check will be blocked implicitly if needed # 2. Process newly received Instant Actions (can potentially happen even if a timed action is running) if self.instant_actions is not None: current_instant_actions = self.instant_actions.instant_actions self.instant_actions = None # Consume the instant actions for action in current_instant_actions: action_started = self.run_action(action) if action_started and self.action_start_time is not None: # If the instant action was timed and started, block further iteration this tick is_action_running = True break # Process one instant action start per tick maybe? # --- >> Check order only if no blocking action is running << --- if self.order is None: return # If a timed action just finished, or no timed action was running, proceed. # If a new timed action just started (from instant actions), maybe block movement. # Let's refine: only proceed to movement if no action is currently RUNNING can_move = True for a_state in self.state.action_states: if a_state.action_status == ActionStatus.RUNNING: can_move = False # print(f"DEBUG ({self.config.vehicle.serial_number}): Action {a_state.action_id} is RUNNING, movement blocked.") break if not can_move: return # Block movement if any action is running # 3. Check if Actions at the current node need starting actions_at_current_node = [] for node in self.order.nodes: if node.sequence_id == self.state.last_node_sequence_id: actions_at_current_node = node.actions break action_started_this_tick = False if actions_at_current_node: for action in actions_at_current_node: # Find corresponding action state for a_state in self.state.action_states: if a_state.action_id == action.action_id and a_state.action_status == ActionStatus.WAITING: # Try to start the action if self.run_action(action): action_started_this_tick = True # If the action has a duration (like dropOff), state_iterate will pause in the next tick due to action_start_time # If the action is HARD blocking, we should not attempt movement this tick if action.blocking_type == BlockingType.HARD: print(f"DEBUG ({self.config.vehicle.serial_number}): HARD blocking action {action.action_id} started at node {self.state.last_node_id}. Halting iteration.") return # Stop processing for this tick # If we just started an action at the current node, don't try to move away yet if action_started_this_tick: # print(f"DEBUG ({self.config.vehicle.serial_number}): Action started at node {self.state.last_node_id}, delaying movement check.") return # Check vehicle position if self.state.agv_position is None: print(f"DEBUG ({self.config.vehicle.serial_number}): No AGV position, cannot move.") return # Check if order is completed (no more nodes except potentially the last one) if len(self.state.node_states) <= 1: if len(self.state.node_states) == 1 and self.state.node_states[0].sequence_id == self.state.last_node_sequence_id: # Only the last completed node remains # print(f"DEBUG ({self.config.vehicle.serial_number}): Order likely completed, last node was {self.state.last_node_id}") # Optionally clear the order state here or wait for new order # self.order = None # Example: Clear order on completion return elif len(self.state.node_states) == 0: # No nodes left # print(f"DEBUG ({self.config.vehicle.serial_number}): No nodes left in state.") return vehicle_position = self.state.agv_position last_node_index = None # Find index of the node we *just completed* or are starting at for i, node_state in enumerate(self.state.node_states): if node_state.sequence_id == self.state.last_node_sequence_id: last_node_index = i break # If the last completed node isn't found, or it's the very last node in the list -> cannot determine next node if last_node_index is None or last_node_index >= len(self.state.node_states) - 1: # print(f"DEBUG ({self.config.vehicle.serial_number}): Cannot determine next node. last_node_index={last_node_index}, node_states_len={len(self.state.node_states)}") # If last_node_index is None, maybe the state is inconsistent # If it's the last node, the order might be finished # Check if only one node left and it's the last one if len(self.state.node_states) == 1 and last_node_index == 0: # print(f"DEBUG ({self.config.vehicle.serial_number}): Reached final node {self.state.node_states[0].node_id}.") pass # Allow potential actions on the final node to run if any added later return # Cannot move further # Determine the next node to move towards next_node_state = self.state.node_states[last_node_index + 1] if next_node_state.node_position is None: print(f"DEBUG ({self.config.vehicle.serial_number}): Next node {next_node_state.node_id} has no position!") return # Cannot move to a node without a position next_node_position = next_node_state.node_position # print(f"DEBUG ({self.config.vehicle.serial_number}): Target node {next_node_state.node_id} at ({next_node_position.x:.2f}, {next_node_position.y:.2f})") # --- Calculate and Update Position --- current_x = vehicle_position.x current_y = vehicle_position.y target_x = next_node_position.x target_y = next_node_position.y # Check distance to see if we are already there (or very close) distance_to_next_node = utils.get_distance( current_x, current_y, target_x, target_y ) arrival_threshold = self.config.settings.speed * 0.5 + 0.05 # Adjust threshold: half tick + buffer if distance_to_next_node < arrival_threshold: # Arrived at the next node print(f"SIM ({self.config.vehicle.serial_number}): Arrived at node {next_node_state.node_id} (Seq: {next_node_state.sequence_id})") # Update state to reflect arrival self.state.agv_position.x = target_x # Snap to exact node position on arrival self.state.agv_position.y = target_y # Optionally update theta if node has orientation self.state.last_node_id = next_node_state.node_id self.state.last_node_sequence_id = next_node_state.sequence_id # Remove the corresponding edge state (find by end_node_id and sequence_id?) # VDA edge sequence might differ, safer to find by end_node_id matching the arrived node edge_to_remove_idx = -1 for idx, edge in enumerate(self.state.edge_states): if edge.end_node_id == next_node_state.node_id: edge_to_remove_idx = idx break if edge_to_remove_idx != -1: removed_edge = self.state.edge_states.pop(edge_to_remove_idx) # print(f"DEBUG ({self.config.vehicle.serial_number}): Removed edge {removed_edge.edge_id}") # IMPORTANT: Do not remove the node state we just arrived at yet! # Actions associated with this node need to be checked/run in the *next* state_iterate cycle. # Update visualization state self.visualization.agv_position = self.state.agv_position else: # --- Move towards the next node --- self.state.driving = True # print(f"SIMULATOR ({self.config.vehicle.serial_number}): Moving from ({current_x:.2f}, {current_y:.2f}) towards ({target_x:.2f}, {target_y:.2f}).") updated_vehicle_position = utils.iterate_position( current_x, current_y, target_x, target_y, self.config.settings.speed ) self.state.agv_position.x = updated_vehicle_position[0] self.state.agv_position.y = updated_vehicle_position[1] self.state.agv_position.theta = updated_vehicle_position[2] # print(f"SIMULATOR ({self.config.vehicle.serial_number}): AGV position updated to: ({self.state.agv_position.x:.2f}, {self.state.agv_position.y:.2f})") # Update visualization state self.visualization.agv_position = self.state.agv_position class MQTTClient: """MQTT client for VDA5050 communication""" def __init__(self, config_data: config.Config): self.config = config_data self.client = None self.base_topic = mqtt_utils.generate_vda_mqtt_base_topic( self.config.mqtt_broker.vda_interface, self.config.vehicle.vda_version, self.config.vehicle.manufacturer, self.config.vehicle.serial_number ) self.message_queue = None # Will be created in connect self.loop = None def on_connect(self, client, userdata, flags, rc, properties=None): """Callback when connected to MQTT broker""" print(f"Connected with result code {rc}") # Subscribe to topics topics = [ f"{self.base_topic}/order", f"{self.base_topic}/instantActions" ] for topic in topics: client.subscribe(topic, qos=1) print(f"SIM ({self.config.vehicle.serial_number}): Subscribed to {topic}") def on_message(self, client, userdata, msg): """Callback when message is received (Runs in MQTT Thread)""" topic = msg.topic topic_type = utils.get_topic_type(topic) payload = msg.payload.decode('utf-8') print(f"SIM ({self.config.vehicle.serial_number}): Received raw message on {topic_type}") # Added serial for clarity if self.loop is None or self.message_queue is None: print(f"SIM ({self.config.vehicle.serial_number}): Error - Loop or Queue not initialized in on_message.") return try: if topic_type == "order": order_data = json.loads(payload) order = Order.from_dict(order_data) # Safely put item onto the asyncio queue from this thread self.loop.call_soon_threadsafe(self.message_queue.put_nowait, ("order", order)) print(f"SIM ({self.config.vehicle.serial_number}): Queued order {order.order_id}") elif topic_type == "instantActions": instant_actions_data = json.loads(payload) instant_actions = InstantActions.from_dict(instant_actions_data) # Safely put item onto the asyncio queue from this thread self.loop.call_soon_threadsafe(self.message_queue.put_nowait, ("instantActions", instant_actions)) print(f"SIM ({self.config.vehicle.serial_number}): Queued instant actions") except Exception as e: print(f"SIM ({self.config.vehicle.serial_number}): Error processing message on {topic}: {e}") def connect(self): """Connect to MQTT broker""" opts = mqtt_utils.mqtt_create_opts() # Use unique client ID for each simulator instance client_id_suffix = self.config.vehicle.serial_number.replace(" ", "_") # Make suffix safe for client ID self.client = mqtt.Client(client_id=f"{opts['client_id']}-{client_id_suffix}", protocol=mqtt.MQTTv5) # Set callbacks self.client.on_connect = self.on_connect self.client.on_message = self.on_message # Get the running asyncio event loop self.loop = asyncio.get_running_loop() # Create the asyncio queue (needs the loop) self.message_queue = asyncio.Queue() # Connect to broker self.client.connect( host=self.config.mqtt_broker.host, port=int(self.config.mqtt_broker.port) ) # Start the loop (in a separate thread) self.client.loop_start() # Return the queue for the main async logic return self.client, self.message_queue async def subscribe_vda_messages(vehicle_simulator, mqtt_client, message_queue): """Process messages from MQTT broker""" while True: try: # Get message from queue message_type, message_data = await message_queue.get() if message_type == "order": vehicle_simulator.order_accept_procedure(message_data) elif message_type == "instantActions": vehicle_simulator.instant_actions_accept_procedure(message_data) # Mark task as done message_queue.task_done() except Exception as e: print(f"Error processing message: {e}") await asyncio.sleep(1) async def publish_vda_messages(vehicle_simulator, mqtt_client, state_frequency, visualization_frequency): """Publish messages to MQTT broker""" # First publish connection state await vehicle_simulator.publish_connection(mqtt_client) # Calculate tick time (50ms) tick_time = 0.05 counter_state = 0 counter_visualization = 0 while True: # Update vehicle state vehicle_simulator.state_iterate() # Check if it's time to publish state counter_state += 1 if counter_state * tick_time > 1.0 / state_frequency: counter_state = 0 await vehicle_simulator.publish_state(mqtt_client) # Check if it's time to publish visualization counter_visualization += 1 if counter_visualization * tick_time > 1.0 / visualization_frequency: counter_visualization = 0 await vehicle_simulator.publish_visualization(mqtt_client) # Sleep for tick time await asyncio.sleep(tick_time) async def main(): """Main function""" # Load configuration config_data = config.get_config() # Create tasks for each robot tasks = [] for robot_index in range(config_data.settings.robot_count): # Clone generic config vehicle_config = config_data # Rename robot serial number vehicle_config.vehicle.serial_number = f"{config_data.vehicle.serial_number}{robot_index}" # Create vehicle simulator vehicle_simulator = VehicleSimulator(vehicle_config) # Create MQTT client mqtt_handler = MQTTClient(vehicle_config) mqtt_client, message_queue = mqtt_handler.connect() # Create tasks subscribe_task = asyncio.create_task( subscribe_vda_messages(vehicle_simulator, mqtt_client, message_queue) ) publish_task = asyncio.create_task( publish_vda_messages( vehicle_simulator, mqtt_client, config_data.settings.state_frequency, config_data.settings.visualization_frequency ) ) tasks.extend([subscribe_task, publish_task]) # Wait for all tasks await asyncio.gather(*tasks) if __name__ == "__main__": asyncio.run(main())