diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index 370bc890..da24913e 100644
--- a/.github/workflows/tests.yml
+++ b/.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 }}
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index f0250752..f9ee7821 100644
--- a/CONTRIBUTING.md
+++ b/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.
diff --git a/docker-compose.yaml b/docker-compose.yaml
index 147c08ec..c9cf00c5 100644
--- a/docker-compose.yaml
+++ b/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 #
diff --git a/pyrobosim_ros/CMakeLists.txt b/pyrobosim_ros/CMakeLists.txt
index a49ad050..aebe72e0 100644
--- a/pyrobosim_ros/CMakeLists.txt
+++ b/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()
diff --git a/pyrobosim_ros/package.xml b/pyrobosim_ros/package.xml
index 32e55a2c..c6a046ea 100644
--- a/pyrobosim_ros/package.xml
+++ b/pyrobosim_ros/package.xml
@@ -20,6 +20,7 @@
pyrobosim_msgs
+ ament_cmake_pytest
ament_copyright
ament_flake8
ament_pep257
diff --git a/pyrobosim_ros/pyrobosim_ros/ros_conversions.py b/pyrobosim_ros/pyrobosim_ros/ros_conversions.py
index 592d7097..f6e0713d 100644
--- a/pyrobosim_ros/pyrobosim_ros/ros_conversions.py
+++ b/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,
)
diff --git a/pyrobosim_ros/test/CMakeLists.txt b/pyrobosim_ros/test/CMakeLists.txt
new file mode 100644
index 00000000..b13d9ab3
--- /dev/null
+++ b/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()
diff --git a/pyrobosim_ros/test/test_ros_conversions.py b/pyrobosim_ros/test/test_ros_conversions.py
new file mode 100644
index 00000000..30fb2036
--- /dev/null
+++ b/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)
diff --git a/test/run_tests.bash b/test/run_tests.bash
index 2728b141..6b709830 100755
--- a/test/run_tests.bash
+++ b/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