Add basic colcon tests to pyrobosim_ros package (#124)

.github/workflows/tests.yml

@@ -47,7 +47,7 @@ jobs:
   ros2-test:
     strategy:
       matrix:
-        ros_distro: [humble, iron]
+        ros_distro: [humble, iron, rolling]
     env:
       ROS_DISTRO: ${{ matrix.ros_distro }}
 

CONTRIBUTING.md

@@ -39,7 +39,7 @@ You can run all unit tests locally before you push to your branch and wait for C
 To do this:
 
 * Go to the root folder of this repository.
-* Run `test/run_tests.bash`
+* Run `test/run_tests.bash` (or `test/run_tests.bash $ROS_DISTRO` if using ROS)
 
 The above script runs [`pytest`](https://docs.pytest.org/) with the settings configured for this test.
 You can also look at, and modify, the `pytest.ini` file at the root of this repository.

docker-compose.yaml

@@ -44,7 +44,7 @@ services:
 
   test:
     extends: base
-    command: test/run_tests.bash
+    command: test/run_tests.bash ${ROS_DISTRO:-humble}
 
   ###############
   # Basic demos #

pyrobosim_ros/CMakeLists.txt

@@ -45,4 +45,9 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}
 )
 
+# Build tests if enabled
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
 ament_package()

pyrobosim_ros/package.xml

@@ -20,6 +20,7 @@
   <exec_depend>pyrobosim_msgs</exec_depend>
 
   <!-- Test dependencies -->
+  <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

pyrobosim_ros/pyrobosim_ros/ros_conversions.py

@@ -138,15 +138,14 @@ def task_action_from_ros(msg):
     """
     if not isinstance(msg, ros_msgs.TaskAction):
         raise Exception("Input is not a TaskAction ROS message")
-    pose = pose_from_ros(msg.pose) if msg.has_pose else None
     return acts.TaskAction(
         msg.type,
-        robot=msg.robot,
-        object=msg.object,
-        room=msg.room,
-        source_location=msg.source_location,
-        target_location=msg.target_location,
-        pose=pose,
+        robot=msg.robot if msg.robot else None,
+        object=msg.object if msg.object else None,
+        room=msg.room if msg.room else None,
+        source_location=msg.source_location if msg.source_location else None,
+        target_location=msg.target_location if msg.target_location else None,
+        pose=pose_from_ros(msg.pose) if msg.has_pose else None,
         path=path_from_ros(msg.path),
         cost=msg.cost,
     )

pyrobosim_ros/test/CMakeLists.txt

@@ -0,0 +1,14 @@
+# CMakeLists.txt for pyrobosim_ros tests
+
+find_package(ament_cmake_pytest REQUIRED)
+set(_pytest_tests
+    test_ros_conversions.py
+)
+foreach(_test_path ${_pytest_tests})
+get_filename_component(_test_name ${_test_path} NAME_WE)
+ament_add_pytest_test(${_test_name} ${_test_path}
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+    TIMEOUT 60
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+)
+endforeach()

pyrobosim_ros/test/test_ros_conversions.py

@@ -0,0 +1,170 @@
+# Tests for pyrobosim ROS conversions functionality
+
+import pytest
+
+import pyrobosim_msgs.msg as ros_msgs
+import pyrobosim.planning.actions as acts
+from pyrobosim.utils.motion import Path
+from pyrobosim.utils.pose import Pose
+from pyrobosim_ros.ros_conversions import (
+    path_from_ros,
+    path_to_ros,
+    pose_from_ros,
+    pose_to_ros,
+    task_action_from_ros,
+    task_action_to_ros,
+    task_plan_from_ros,
+    task_plan_to_ros,
+)
+
+
+def test_pose_conversion():
+    """Tests round-trip conversion of pose objects."""
+    # Create a pyrobosim pose
+    orig_pose = Pose(x=1.0, y=2.0, z=3.0, q=[0.707, 0.0, 0.707, 0])  # wxyz
+
+    # Convert to a ROS Message
+    ros_pose = pose_to_ros(orig_pose)
+    assert ros_pose.position.x == pytest.approx(orig_pose.x)
+    assert ros_pose.position.y == pytest.approx(orig_pose.y)
+    assert ros_pose.position.z == pytest.approx(orig_pose.z)
+    assert ros_pose.orientation.w == pytest.approx(orig_pose.q[0])
+    assert ros_pose.orientation.x == pytest.approx(orig_pose.q[1])
+    assert ros_pose.orientation.y == pytest.approx(orig_pose.q[2])
+    assert ros_pose.orientation.z == pytest.approx(orig_pose.q[3])
+
+    # Convert back to a pyrobosim Pose
+    new_pose = pose_from_ros(ros_pose)
+    assert new_pose.x == pytest.approx(orig_pose.x)
+    assert new_pose.y == pytest.approx(orig_pose.y)
+    assert new_pose.z == pytest.approx(orig_pose.z)
+    assert new_pose.q == pytest.approx(orig_pose.q)
+
+
+def test_path_conversion():
+    """Tests round-trip conversion of path objects."""
+
+    # Create a pyrobosim path
+    poses = [
+        Pose(x=0.0, y=0.0, z=0.0, q=[1.0, 0.0, 0.0, 0.0]),
+        Pose(x=1.0, y=0.0, z=0.0, q=[0.707, 0.0, 0.0, 0.707]),
+        Pose(x=1.0, y=1.0, z=0.0, q=[0.0, 0.0, 0.0, 1.0]),
+    ]
+    orig_path = Path(poses)
+
+    # Convert to a ROS Message
+    ros_path = path_to_ros(orig_path)
+    assert len(ros_path.poses) == orig_path.num_poses
+    for orig_pose, ros_pose in zip(orig_path.poses, ros_path.poses):
+        assert ros_pose.position.x == pytest.approx(orig_pose.x)
+        assert ros_pose.position.y == pytest.approx(orig_pose.y)
+        assert ros_pose.position.z == pytest.approx(orig_pose.z)
+        assert ros_pose.orientation.w == pytest.approx(orig_pose.q[0])
+        assert ros_pose.orientation.x == pytest.approx(orig_pose.q[1])
+        assert ros_pose.orientation.y == pytest.approx(orig_pose.q[2])
+        assert ros_pose.orientation.z == pytest.approx(orig_pose.q[3])
+
+    # Convert back to a pyrobosim path
+    new_path = path_from_ros(ros_path)
+    assert new_path.num_poses == orig_path.num_poses
+    for orig_pose, new_pose in zip(orig_path.poses, new_path.poses):
+        assert orig_pose.x == pytest.approx(new_pose.x)
+        assert orig_pose.y == pytest.approx(new_pose.y)
+        assert orig_pose.z == pytest.approx(new_pose.z)
+        assert orig_pose.q == pytest.approx(new_pose.q)
+
+
+def test_task_action_conversion():
+    """Tests round-trip conversion of task action objects."""
+
+    # Create a pyrobosim task action
+    orig_action = acts.TaskAction(
+        "pick",
+        robot="robot0",
+        object="apple",
+        room="kitchen",
+        source_location="table",
+        target_location=None,
+        pose=Pose(x=1.0, y=2.0, z=3.0, q=[1.0, 0.0, 0.0, 0.0]),
+        path=Path(),
+        cost=42.0,
+    )
+
+    # Convert to a ROS message
+    ros_action = task_action_to_ros(orig_action)
+    assert ros_action.robot == orig_action.robot
+    assert ros_action.type == orig_action.type
+    assert ros_action.object == orig_action.object
+    assert ros_action.room == orig_action.room
+    assert ros_action.source_location == orig_action.source_location
+    assert ros_action.target_location == ""  # No None in ROS messages
+    assert ros_action.cost == pytest.approx(orig_action.cost)
+    assert ros_action.has_pose
+    assert ros_action.pose.position.x == pytest.approx(orig_action.pose.x)
+    assert ros_action.pose.position.y == pytest.approx(orig_action.pose.y)
+    assert ros_action.pose.position.z == pytest.approx(orig_action.pose.z)
+    assert ros_action.pose.orientation.w == pytest.approx(orig_action.pose.q[0])
+    assert ros_action.pose.orientation.x == pytest.approx(orig_action.pose.q[1])
+    assert ros_action.pose.orientation.y == pytest.approx(orig_action.pose.q[2])
+    assert ros_action.pose.orientation.z == pytest.approx(orig_action.pose.q[3])
+    assert len(ros_action.path.poses) == orig_action.path.num_poses
+
+    # Convert back to a pyrobosim task action
+    new_action = task_action_from_ros(ros_action)
+    assert new_action.robot == orig_action.robot
+    assert new_action.type == orig_action.type
+    assert new_action.object == orig_action.object
+    assert new_action.room == orig_action.room
+    assert new_action.source_location == orig_action.source_location
+    assert new_action.target_location == orig_action.target_location
+    assert new_action.cost == pytest.approx(orig_action.cost)
+    assert new_action.pose is not None
+    assert new_action.pose.x == pytest.approx(orig_action.pose.x)
+    assert new_action.pose.y == pytest.approx(orig_action.pose.y)
+    assert new_action.pose.z == pytest.approx(orig_action.pose.z)
+    assert new_action.pose.q == pytest.approx(orig_action.pose.q)
+    assert new_action.path.num_poses == orig_action.path.num_poses
+
+
+def test_task_plan_conversion():
+    """Tests round-trip conversion of task plan objects."""
+
+    # Create a pyrobosim task plan
+    nav_path = Path(
+        [
+            Pose(x=0.0, y=0.0, z=0.0, q=[1.0, 0.0, 0.0, 0.0]),
+            Pose(x=1.0, y=0.0, z=0.0, q=[0.707, 0.0, 0.0, 0.707]),
+            Pose(x=1.0, y=1.0, z=0.0, q=[0.0, 0.0, 0.0, 1.0]),
+        ]
+    )
+    place_pose = Pose(x=0.8, y=1.0, z=0.5, q=[1.0, 0.0, 0.0, 0.0])
+    actions = [
+        acts.TaskAction("pick", object="apple", source_location="table0", cost=0.5),
+        acts.TaskAction(
+            "move",
+            source_location="table0",
+            target_location="desk0",
+            path=nav_path,
+            cost=0.75,
+        ),
+        acts.TaskAction(
+            "place", object="apple", target_location="desk0", pose=place_pose, cost=0.5
+        ),
+    ]
+    orig_plan = acts.TaskPlan(robot="robot0", actions=actions)
+
+    # Convert to a ROS message
+    ros_plan = task_plan_to_ros(orig_plan)
+    assert ros_plan.robot == orig_plan.robot
+    assert len(ros_plan.actions) == len(orig_plan.actions)
+    for orig_action, ros_action in zip(orig_plan.actions, ros_plan.actions):
+        assert orig_action.type == ros_action.type
+    assert ros_plan.cost == pytest.approx(orig_plan.total_cost)
+
+    # Convert back to a pyrobosim task action
+    new_plan = task_plan_from_ros(ros_plan)
+    assert new_plan.robot == orig_plan.robot
+    assert len(new_plan.actions) == len(orig_plan.actions)
+    for orig_action, new_action in zip(orig_plan.actions, new_plan.actions):
+        assert orig_action.type == new_action.type
+    assert new_plan.total_cost == pytest.approx(orig_plan.total_cost)

test/run_tests.bash

@@ -1,11 +1,32 @@
 #!/bin/bash
 
 # Runs all unit tests
-echo "Running unit tests..."
+#
+# If not using ROS, simply run
+#   ./run_tests.bash
+#
+# If using ROS, additionally pass in your ROS_DISTRO argument (e.g., humble or rolling)
+#   ./run_tests.bash ${ROS_DISTRO}
 
 SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
-TEST_RESULTS_DIR=$SCRIPT_DIR/results
+TEST_RESULTS_DIR="${SCRIPT_DIR}/results"
 
+# Run regular pytest tests
+echo "Running Python package unit tests..."
 python3 -m pytest "$SCRIPT_DIR" \
  --junitxml="$TEST_RESULTS_DIR/test_results.xml" \
  --html="$TEST_RESULTS_DIR/test_results.html" --self-contained-html
+
+echo ""
+
+# Run colcon tests, if using a ROS distro
+ROS_DISTRO=$1
+if [[ -n "$ROS_DISTRO" && -n "$COLCON_PREFIX_PATH" ]]
+then
+    WORKSPACE_DIR="${COLCON_PREFIX_PATH}/../"
+    echo "Running ROS package unit tests from ${WORKSPACE_DIR}..."
+    pushd "${WORKSPACE_DIR}" > /dev/null || exit
+    colcon test --event-handlers console_cohesion+
+    colcon test-result --verbose
+    popd > /dev/null || exit
+fi