diff --git a/vda5050_connector/README.md b/vda5050_connector/README.md
index 4264031..95c7e17 100644
--- a/vda5050_connector/README.md
+++ b/vda5050_connector/README.md
@@ -39,23 +39,33 @@ To do this this, you can check the [vda5050_examples](https://github.com/inorbit
### Compilation
-Download `vda5050_msgs` package into your ROS2 workspace
+Start a docker container with the required packages mounted on `dev_ws`:
```bash
-cd ~/dev_ws/ &&
-git clone --branch ros2-vda5050-v2 https://github.com/ipa320/vda5050_msgs.git \
- && mv vda5050_msgs/vda5050_msgs ./src/vda5050_msgs \
- && mv vda5050_msgs/vda5050_serializer ./src/vda5050_serializer \
- && rm -rf vda5050_msgs
+# Go to the repository root folder and execute
+docker run -ti --rm \
+ --workdir /dev_ws/ \
+ -v ./vda5050_msgs:/dev_ws/src/vda5050_msgs \
+ -v ./vda5050_serializer:/dev_ws/src/vda5050_serializer \
+ -v ./vda5050_connector:/dev_ws/src/vda5050_connector \
+ osrf/ros:humble-desktop-full
```
-Then download this package into the ROS2 workspace `src` folder and compile it using `colcon build`.
+Now build the package:
+
+```bash
+apt update && apt install ros-humble-ament* -y
+cd /dev_ws/
+colcon build --symlink-install
+```
### Testing
The package provides several lint test (`ament_pep257` and `ament_flake8` for Python, `ament_clang_format` for C++ and `ament_copyright`), as well as specific unit tests to check the correct functionality of the different modules. To run the tests, execute:
```sh
+cd /dev_ws/
+rosdep install --from-paths src -y --ignore-src
colcon test --packages-select vda5050_connector && colcon test-result --verbose
```
diff --git a/vda5050_connector/package.xml b/vda5050_connector/package.xml
index b38303b..705d9da 100644
--- a/vda5050_connector/package.xml
+++ b/vda5050_connector/package.xml
@@ -40,6 +40,7 @@
ament_flake8
ament_pep257
python3-pytest
+ python3-pytest-mock
ament_cmake
diff --git a/vda5050_connector/test/conftest.py b/vda5050_connector/test/conftest.py
index 865f6f2..dc3fdc2 100644
--- a/vda5050_connector/test/conftest.py
+++ b/vda5050_connector/test/conftest.py
@@ -146,29 +146,29 @@ def publish_feedback(self, feedback):
self.feedback_pub.publish(feedback_message)
-@pytest.fixture()
+@pytest.fixture
def setup_rclpy():
rclpy.init()
yield
rclpy.shutdown()
-@pytest.fixture()
+@pytest.fixture
def adapter_node(setup_rclpy):
return rclpy.create_node("FakeAdapter")
-@pytest.fixture()
+@pytest.fixture
def action_server_nav_to_node(adapter_node):
return MockActionServerNavigateToNode(adapter_node)
-@pytest.fixture()
+@pytest.fixture
def action_server_process_vda_action(adapter_node):
return MockActionServerProcessVDAAction(adapter_node)
-@pytest.fixture()
+@pytest.fixture
def service_get_state(adapter_node):
return adapter_node.create_service(
GetState,
@@ -177,7 +177,7 @@ def service_get_state(adapter_node):
)
-@pytest.fixture()
+@pytest.fixture
def service_supported_actions(adapter_node):
return adapter_node.create_service(
SupportedActions,
diff --git a/vda5050_connector/test/test_vda5050_controller.py b/vda5050_connector/test/test_vda5050_controller.py
index 60deca9..5105db1 100644
--- a/vda5050_connector/test/test_vda5050_controller.py
+++ b/vda5050_connector/test/test_vda5050_controller.py
@@ -30,7 +30,6 @@
# POSSIBILITY OF SUCH DAMAGE.
import rclpy
-import copy
from rclpy.logging import LoggingSeverity
from rclpy.task import Future
@@ -46,6 +45,8 @@
from vda5050_msgs.msg import Node
from vda5050_msgs.msg import Edge
from vda5050_msgs.msg import NodePosition
+from vda5050_msgs.msg import Action
+from vda5050_msgs.msg import ActionParameter
def get_order_new(order_id=str(uuid4()), order_update_id=0):
@@ -221,6 +222,149 @@ def get_order_update(order_id=str(uuid4()), order_update_id=0):
)
+def get_stitch_orders(order_id=str(uuid4())):
+ action1 = Action(
+ action_type="foo",
+ action_id=str(uuid4()),
+ action_description="Foo description",
+ blocking_type="NONE",
+ action_parameters=[
+ ActionParameter(key="foo", value="bar")
+ ]
+ )
+ action2 = Action(
+ action_type="bar",
+ action_id=str(uuid4()),
+ action_description="Bar description",
+ blocking_type="NONE",
+ action_parameters=[
+ ActionParameter(key="foo", value="bar")
+ ]
+ )
+ action3 = Action(
+ action_type="foobar",
+ action_id=str(uuid4()),
+ action_description="FooBar description",
+ blocking_type="NONE",
+ action_parameters=[
+ ActionParameter(key="abc", value="xyz")
+ ]
+ )
+ base_order = Order(
+ header_id=0,
+ timestamp=get_vda5050_ts(),
+ version="1.1.1",
+ manufacturer="MANUFACTURER",
+ serial_number="SERIAL_NUMBER",
+ order_id=order_id,
+ order_update_id=0,
+ nodes=[
+ Node(
+ node_id="node1",
+ sequence_id=0,
+ released=True,
+ node_position=NodePosition(
+ x=2.0,
+ y=0.95,
+ theta=-0.66,
+ allowed_deviation_x_y=0.0,
+ allowed_deviation_theta=0.0,
+ map_id="map",
+ ),
+ actions=[
+ action1
+ ],
+ ),
+ Node(
+ node_id="node2",
+ sequence_id=2,
+ released=True,
+ node_position=NodePosition(
+ x=1.18,
+ y=-1.76,
+ theta=0.0,
+ allowed_deviation_x_y=0.0,
+ allowed_deviation_theta=0.0,
+ map_id="map",
+ ),
+ actions=[
+ action2
+ ],
+ ),
+ ],
+ edges=[
+ Edge(
+ edge_id="edge1",
+ sequence_id=1,
+ released=True,
+ start_node_id="node1",
+ end_node_id="node2",
+ max_speed=10.0,
+ max_height=10.0,
+ min_height=1.0,
+ ),
+ ],
+ )
+
+ stitch_order = Order(
+ header_id=0,
+ timestamp=get_vda5050_ts(),
+ version="1.1.1",
+ manufacturer="MANUFACTURER",
+ serial_number="SERIAL_NUMBER",
+ order_id=order_id,
+ order_update_id=1,
+ nodes=[
+ Node(
+ node_id="node2",
+ sequence_id=2,
+ released=True,
+ node_position=NodePosition(
+ x=1.18,
+ y=-1.76,
+ theta=0.0,
+ allowed_deviation_x_y=0.0,
+ allowed_deviation_theta=0.0,
+ map_id="map",
+ ),
+ actions=[
+ action2
+ ],
+ ),
+ Node(
+ node_id="node3",
+ sequence_id=4,
+ released=True,
+ node_position=NodePosition(
+ x=-0.38,
+ y=1.89,
+ theta=0.0,
+ allowed_deviation_x_y=0.0,
+ allowed_deviation_theta=0.0,
+ map_id="map",
+ ),
+ actions=[
+ action3
+ ],
+ ),
+ ],
+ edges=[
+ Edge(
+ edge_id="edge2",
+ sequence_id=3,
+ released=True,
+ start_node_id="node2",
+ end_node_id="node3",
+ max_speed=10.0,
+ max_height=10.0,
+ min_height=1.0,
+ ),
+ ],
+ )
+
+ return [base_order, stitch_order]
+
+
def test_vda5050_controller_node_new_order(
mocker,
adapter_node,
@@ -426,18 +570,17 @@ def test_vda5050_controller_node_stitch_order(
node = VDA5050Controller()
node.logger.set_level(LoggingSeverity.DEBUG)
- # add a spy to validate used navigation goal parameters
- spy_send_adapter_navigate_to_node = mocker.spy(
- node, "send_adapter_navigate_to_node"
- )
-
# add a spy to validate accept order is called correctly
spy_accept_order = mocker.spy(node, "_accept_order")
- # Send first new order
- order_id = str(uuid4())
- order = get_order_new(order_id)
- node.process_order(order)
+ # Get the base order and the stitch order, both with two nodes
+ # and one edge each. The resulting (stitched) order should contain
+ # three nodes and two edges:
+ # (A -> B) + (B -> C) = A -> B -> C
+ # All three nodes have an action.
+ [base_order, stitch_order] = get_stitch_orders()
+
+ node.process_order(base_order)
rclpy.spin_once(node)
rclpy.spin_once(adapter_node)
@@ -446,70 +589,39 @@ def test_vda5050_controller_node_stitch_order(
future = Future()
future.set_result(result=NavigateToNode.Result())
- # The NEW order contains 5 nodes and 4 edges. The first node (in deviation range)
- # is processed and remove, and 3 nodes are send to navigate to.
- node._navigate_to_node_result_callback(future)
- node._on_active_order()
- node._navigate_to_node_result_callback(future)
- node._on_active_order()
- node._navigate_to_node_result_callback(future)
- node._on_active_order()
-
- spy_accept_order.reset_mock()
- spy_send_adapter_navigate_to_node.reset_mock()
-
- # Send update order before the first is finished
- stitched_order = copy.deepcopy(order)
- order = get_order_update(order_id, 1) # Same order id
-
- # Create Stitched order from the previous order and the update
- # Remove the last node as it's in the update
- stitched_order.nodes.pop()
- # Add the new updates
- stitched_order.nodes += copy.deepcopy(order.nodes)
- stitched_order.edges += copy.deepcopy(order.edges)
- stitched_order.order_update_id = copy.deepcopy(order.order_update_id)
- stitched_order.zone_set_id = copy.deepcopy(order.zone_set_id)
-
- node.process_order(order)
-
+ # The base order contains 2 nodes and 1 edge. The first node (in deviation range)
+ # is processed and removed, and 1 node is sent to navigate to.
node._navigate_to_node_result_callback(future)
- node._on_active_order()
- spy_accept_order.assert_called_once_with(order=order, mode=OrderAcceptModes.STITCH)
+ assert len(node._current_order.nodes) == 2
+ assert len(node._current_order.edges) == 1
+ assert len(node._current_state.action_states) == 2
- # check node states were properly updated
- assert node._current_order == stitched_order
+ spy_accept_order.reset_mock()
+ # Process the stitching order
+ node.process_order(stitch_order)
+ spy_accept_order.assert_called_once_with(order=stitch_order, mode=OrderAcceptModes.STITCH)
- assert node._current_state.order_id == order_id
+ assert node._current_state.order_id == stitch_order.order_id
assert node._current_state.order_update_id == 1
- # The order has 2 nodes and 1 edges but the first edge and node
- # are processed as soon as the order is accepted.
- assert len(node._current_state.node_states) == 1
- assert len(node._current_state.edge_states) == 1
-
- assert node._current_state.last_node_id == "node1"
- assert node._current_state.last_node_sequence_id == 8
+ assert len(node._current_order.nodes) == 3
+ assert len(node._current_order.edges) == 2
- # Assert the first navigation goal was sent to the adapter,
- # and that the parameters matches order's first edge and second node.
- # Note: the standard assumes the vehicle is on the first node already,
- # so the first navigation command is to the second order node.
- spy_send_adapter_navigate_to_node.assert_called_once_with(
- edge=order.edges[0], node=order.nodes[1]
- )
+ assert len(node._current_state.node_states) == 2
+ assert len(node._current_state.edge_states) == 1
+ assert len(node._current_state.action_states) == 3
- # Future for invoking adapter navigation goal result callback
- future = Future()
- future.set_result(result=NavigateToNode.Result())
+ assert node._current_state.last_node_id == "node2"
+ assert node._current_state.last_node_sequence_id == 2
- # Simulate the adapter reached navigation goal
node._navigate_to_node_result_callback(future)
- assert len(node._current_state.node_states) == 0
+ assert len(node._current_state.node_states) == 1
assert len(node._current_state.edge_states) == 0
- assert node._current_state.last_node_id == "node2"
- assert node._current_state.last_node_sequence_id == 10
+ assert len(node._current_state.action_states) == 3
+
+ assert node._current_state.last_node_id == "node3"
+ assert node._current_state.last_node_sequence_id == 4
def test_vda5050_controller_node_reject_order(
diff --git a/vda5050_connector/vda5050_connector_py/vda5050_controller.py b/vda5050_connector/vda5050_connector_py/vda5050_controller.py
index 0958c08..16424b4 100755
--- a/vda5050_connector/vda5050_connector_py/vda5050_controller.py
+++ b/vda5050_connector/vda5050_connector_py/vda5050_controller.py
@@ -886,7 +886,7 @@ def process_order(self, order: VDAOrder):
else:
# Same order graph (Same order_id)
update_id_diff = order.order_update_id - self._current_order.order_update_id
- match_last_new_base_nodes = self._match_base_nodes(order)
+ match_last_new_base_nodes = self._match_stitch_nodes(order)
if update_id_diff == 0:
# Same update id, discard the msg
@@ -896,7 +896,7 @@ def process_order(self, order: VDAOrder):
# Reject if update id is lower or if last and new base nodes doesn't match
error_description = (
f"New base start node [{order.nodes[0].node_id}] doesn't match with old base"
- f" last node [{self._current_order.nodes[0].node_id}]"
+ f" last node [{self._current_order.nodes[-1].node_id}]"
if not match_last_new_base_nodes
else (
f"New update id {order.order_update_id} lower than old update id"
@@ -946,6 +946,7 @@ def _has_current_order(self) -> bool:
action_id_cancel = -1
if self._canceling_order():
action_id_cancel = self._cancel_action.action_id
+
has_running_actions = any(
[
action_state.action_status
@@ -954,11 +955,35 @@ def _has_current_order(self) -> bool:
if action_state.action_id != action_id_cancel
]
)
+
# If there are no actions, but there are node / edge states, there is an active order
- return has_running_actions or (
+ has_nodes_and_edges = (
len(self._current_state.node_states) and len(self._current_state.edge_states)
)
+ if has_nodes_and_edges:
+ self.logger.debug((
+ "Found nodes and edges while validating if there's an active order."
+ f" Nodes: {self._current_state.node_states}."
+ f" Edges: {self._current_state.edge_states}."
+ ), throttle_duration_sec=5)
+
+ if has_running_actions:
+ running_actions = [
+ action_state
+ for action_state in self._current_state.action_states
+ if action_state.action_id != action_id_cancel and
+ action_state.action_status not in [
+ VDACurrentAction.FINISHED, VDACurrentAction.FAILED
+ ]
+ ]
+ self.logger.debug((
+ "Found running actions while validating if there's an active order."
+ f" Actions: {running_actions}"
+ ), throttle_duration_sec=5)
+
+ return has_running_actions or has_nodes_and_edges
+
def _first_node_in_deviation_range(self, order: VDAOrder) -> bool:
"""
Validate if first node is in deviation range.
@@ -982,7 +1007,7 @@ def _first_node_in_deviation_range(self, order: VDAOrder) -> bool:
# deviation of theta
return True
- def _match_base_nodes(self, order: VDAOrder):
+ def _match_stitch_nodes(self, order: VDAOrder):
"""
Validate if the last node from the old base matches the first node on the new base.
@@ -995,8 +1020,88 @@ def _match_base_nodes(self, order: VDAOrder):
True if last <> first base nodes match, False otherwise.
"""
- # TODO: Proper implementation
- return True
+ last_node = self._current_order.nodes[-1]
+ stitch_node = order.nodes[0]
+
+ # Return False if node actions differ
+ if len(last_node.actions) != len(stitch_node.actions):
+ self.logger.error("Error while validating stitch node: number of actions don't match")
+ return False
+
+ # Evaluate all actions are equal and in the same order
+ for last_node_actions, stitch_node_actions in zip(last_node.actions, stitch_node.actions):
+ # Return False if action_type, action_id, blocking_type or action_description
+ # of an action are not the same
+ if (
+ last_node_actions.action_type != stitch_node_actions.action_type or
+ last_node_actions.action_id != stitch_node_actions.action_id or
+ last_node_actions.blocking_type != stitch_node_actions.blocking_type or
+ last_node_actions.action_description != stitch_node_actions.action_description
+ ):
+ self.logger.error((
+ "Error while validating stitch node: actions don't match."
+ f" action on last node '{last_node_actions}' differs from "
+ f" action on stitch node '{stitch_node_actions}'"
+ ))
+ return False
+
+ # If the action has different number of parameter return False
+ if (len(last_node_actions.action_parameters) !=
+ len(stitch_node_actions.action_parameters)):
+ self.logger.error((
+ "Error while validating stitch node: Number"
+ " of parameters on node actions differ"
+ ))
+ return False
+
+ # If any of the parameters of an action are not the same return False
+ if any([
+ last_node_action_action_parameter != stitch_node_action_action_parameter
+ for last_node_action_action_parameter, stitch_node_action_action_parameter
+ in zip(last_node_actions.action_parameters, stitch_node_actions.action_parameters)
+ ]):
+ self.logger.error((
+ "Error while validating stitch node: Parameters"
+ " on one of the node actions differ."
+ f" Last node action parameters: '{last_node_actions.action_parameters}'"
+ f" Stitch node action parameters: '{stitch_node_actions.action_parameters}'"
+ ))
+ return False
+
+ # Calculate if node_id and sequence_id matches
+ has_same_node_id = last_node.node_id == stitch_node.node_id
+ has_same_sequence_id = last_node.sequence_id == stitch_node.sequence_id
+
+ if not has_same_node_id or not has_same_sequence_id:
+ self.logger.error(
+ ("Error while validating stitch node: Node ID or Sequence ID mismatch")
+ )
+ # Calculate if both nodes positions are the same
+ last_node_position = last_node.node_position
+ stitch_node_position = stitch_node.node_position
+
+ has_same_node_position = (
+ last_node_position.x == stitch_node_position.x
+ and last_node_position.y == stitch_node_position.y
+ and last_node_position.y == stitch_node_position.y
+ and last_node_position.theta == stitch_node_position.theta
+ and (last_node_position.allowed_deviation_x_y ==
+ stitch_node_position.allowed_deviation_x_y)
+ and (last_node_position.allowed_deviation_theta ==
+ stitch_node_position.allowed_deviation_theta)
+ and last_node_position.map_id == stitch_node_position.map_id
+ and last_node_position.map_description == stitch_node_position.map_description
+ )
+
+ if not has_same_node_position:
+ self.logger.error((
+ "Error while validating stitch node: Node position mismatch."
+ f" Last node position '{last_node_position}'."
+ f" Stitch node position '{stitch_node_position}'."
+ ))
+
+ # Return True if both nodes have same id, sequence_id and position
+ return has_same_node_id and has_same_sequence_id and has_same_node_position
def _accept_order(self, order: VDAOrder, mode: OrderAcceptModes):
"""
@@ -1073,7 +1178,7 @@ def _accept_order(self, order: VDAOrder, mode: OrderAcceptModes):
+ self._get_node_states(order),
"edge_states": (mode == OrderAcceptModes.STITCH) * self._current_state.edge_states
+ self._get_edge_states(order),
- "action_states": self._current_state.action_states
+ "action_states": self._current_state.action_states[:-len(order.nodes[0].actions)]
+ self._get_action_states(order),
"new_base_request": False,
}
@@ -1122,7 +1227,7 @@ def _reject_order(self, order: VDAOrder, error: OrderRejectErrors, description:
order_error.error_type = error.value
order_error.error_description = (
f"VDA5050 order {order.order_id} with update ID"
- f" {order.order_update_id} rejected [{description}]."
+ f" {order.order_update_id} rejected. Reason: {description}"
)
order_error.error_level = VDAError.WARNING
order_error.error_references = error_references