[VDA5050] Order Stitching Seems to Be Getting Wrong Base Last Node

[kurtist123]

I'm trying to issue an order with a horizon of two unreleased nodes, and then issue an order update releasing the last two nodes. My AGV will correctly stop on the last released node of the base. When I send an order update releasing the last two nodes I get the error:

Error while validating stitch node: Node ID or Sequence ID mismatch

Error while validating stitch node: Node position mismatch. Last node position 'vda5050_msgs.msg.NodePosition(x=11.869, y=-0.581, theta=-3.14159, allowed_deviation_x_y=0.0, allowed_deviation_theta=0.0, map_id='0', map_description='')'. Stitch node position 'vda5050_msgs.msg.NodePosition(x=5.484, y=7.355, theta=0.0, allowed_deviation_x_y=0.0, allowed_deviation_theta=0.0, map_id='0', map_description='')'.

Order rejected: VDA5050 order 57f590dc-1be0-4cb5-b7c6-263f152ec699 with update ID 1 rejected. Reason: New base start node [35-39] doesn't match with old base last node [47-IT]

When it grabs the old base last node here: it's just grabbing the last node in the old order. I think it should be grabbing either the last released node or the last traversed node of the old order.

My AGV stops on node [35-39] because it is the last released node in the original order. The last node in the original order is [47-IT] but is not released. The match_stitch_nodes method is grabbing the last node of the order, not the last node of the base.

Is my understanding of what should be happening correct? I can implement a fix if it should be grabbing the last released node the original order.

Other Info:
OS: Ubuntu 22.04
ROS version: ROS2 Humble
Branch: humble-devel