Race Condition in Order Dispatching Leads to Ignored Orders

Description:

We have identified a race condition in the order dispatching logic within our system. The issue arises when the dispatcher sends a new order before the current order's action has fully completed. This results in the second order being ignored because the system is still processing the first one.
Steps to Reproduce:

Dispatch an order with an action that takes some time to complete (e.g., pgv_undock).
Observe that the second order is ignored because the system still considers the first order as "running."

Expected Behavior:

The dispatcher should wait for the first order to fully complete (including all actions and related callbacks) before processing the next order in the queue.
Actual Behavior:

The second order is ignored, leading to potential failures in order execution and workflow disruptions.

[mqtt_to_ros_bridge_node-11] [INFO] [1724638882.785219991] [robot1.mqtt_ros_bridge_node]: From uagv/v2/RobotCompany/robot1/order: b'{"headerId": 0, "timestamp": "2024-08-26T02:21:22.784273", "version": "2.0.0", "manufacturer": "", "serialNumber": "", "orderId": "1-n0", "orderUpdateId": 0, "nodes": [{"nodeId": "1-n0-s0", "sequenceId": 0, "released": true, "nodePosition": {"x": 11.876869932591093, "y": -9.285965825532628, "theta": 1.4213061640766664, "mapId": "", "mapDescription": "", "allowedDeviationXY": 0.0, "allowedDeviationTheta": 0.0}, "actions": [{"actionType": "pgv_undock", "actionId": "1-n0-s0-n0", "blockingType": "HARD", "actionParameters": [{"key": "target_goal", "value": "ato_dock"}], "actionDescription": ""}], "nodeDescription": ""}], "edges": []}'
[vda5050_nav2_client-9] [INFO] [1724638882.785742211] [robot1.nav2_client_node]: Order with order_id 1-n0 received
[vda5050_nav2_client-9] [INFO] [1724638882.792258717] [robot1.nav2_client_node]: Send pgv_undock request
[pgv_docking-6] [INFO] [1724638882.792384092] [robot1.pgv_docking_node]: Received undock goal request with target_goal: ato_dock
[pgv_docking-6] [INFO] [1724638882.792485582] [robot1.pgv_docking_node]: Executing goal
[vda5050_nav2_client-9] [INFO] [1724638882.792501142] [robot1.nav2_client_node]: Action 1-n0-s0-n0 was accepted
[pgv_docking-6] [INFO] [1724638893.692668799] [robot1.pgv_docking_node]: Goal succeeded
[vda5050_nav2_client-9] [INFO] [1724638893.692729931] [robot1.nav2_client_node]: Action 1-n0-s0-n0 was succeeded
[mqtt_to_ros_bridge_node-11] [INFO] [1724638893.705987905] [robot1.mqtt_ros_bridge_node]: From uagv/v2/RobotCompany/robot1/order: b'{"headerId": 1, "timestamp": "2024-08-26T02:21:33.705170", "version": "2.0.0", "manufacturer": "", "serialNumber": "", "orderId": "1-n1", "orderUpdateId": 0, "nodes": [{"nodeId": "1-n1-s0", "sequenceId": 0, "released": true, "nodePosition": {"x": 11.794483758102434, "y": -9.919196267431786, "theta": 1.4181678567834541, "mapId": "", "mapDescription": "", "allowedDeviationXY": 0.0, "allowedDeviationTheta": 0.0}, "actions": [], "nodeDescription": ""}, {"nodeId": "1-n1-s2", "sequenceId": 2, "released": true, "nodePosition": {"x": 8.3, "y": -9.26, "theta": 0.0, "mapId": "map", "mapDescription": "", "allowedDeviationXY": 0.0, "allowedDeviationTheta": 0.0}, "actions": [], "nodeDescription": ""}], "edges": [{"edgeId": "1-e1", "sequenceId": 1, "edgeDescription": "", "released": true, "startNodeId": "1-n1-s0", "endNodeId": "1-n1-s2", "actions": []}]}'
[vda5050_nav2_client-9] [INFO] [1724638893.706719631] [robot1.nav2_client_node]: Order with order_id 1-n1 received
[vda5050_nav2_client-9] [INFO] [1724638893.706739277] [robot1.nav2_client_node]: One order is running. Order 1-n1 is ignored
[vda5050_nav2_client-9] [INFO] [1724638893.792476262] [robot1.nav2_client_node]: Finished action: 1-n0-s0-n0
[vda5050_nav2_client-9] [INFO] [1724638893.892482869] [robot1.nav2_client_node]: Order 1-n0 is completed.

Start at the nav2 client path that logs order reception and the “One order is running” decision. Trace how the action-succeeded callback leads to “Finished action” and “Order ... is completed,” then reproduce the timing with pgv_undock followed by a second order. Done means the second order is retained and processed after the first order and its callbacks fully complete.