Add unittests | Fix bugs

CMakeLists.txt

@@ -43,6 +43,19 @@ if(BUILD_TESTING)
   ament_add_gtest(test_eigen_conversions tests/test_eigen_conversions.cpp
                   tests/gtest_main.cpp)
   target_link_libraries(test_eigen_conversions ${PROJECT_NAME}_conversion)
+
+  find_package(ament_cmake_pytest REQUIRED)
+  set(_pytest_tests
+    tests/test_numpy_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()
 endif()
 
 #

linear_feedback_controller_msgs_py/lfc_py_types.py

@@ -13,7 +13,7 @@ class JointState:
     sensor_msgs.msg.JointState.
     """
 
-    name: str
+    name: List[str]
     position: npt.NDArray[np.float64]
     velocity: npt.NDArray[np.float64]
     effort: npt.NDArray[np.float64]

linear_feedback_controller_msgs_py/numpy_conversions.py

@@ -7,7 +7,7 @@
 from geometry_msgs.msg import Pose, Point, Quaternion, Twist, Vector3, Wrench
 from sensor_msgs.msg import JointState
 
-import lfc_py_types
+import linear_feedback_controller_msgs_py.lfc_py_types as lfc_py_types
 
 from linear_feedback_controller_msgs.msg import Contact, Control, Sensor
 
@@ -26,61 +26,81 @@ def vector3_numpy_to_msg(input: np_array3) -> Vector3:
     Returns:
         geometry_msgs.msg.Vector3: Vector represented as a ROS message.
     """
-    assert len(input.shape) == 3, "Input vector is not of a length 3."
+    assert (
+        input.ndim == 1
+    ), f"Input vector has '{input.ndim}' dimensions, expected dimension size of 1!"
+    assert (
+        input.size == 3
+    ), f"Input vector has length of '{input.size}', expected length '3'!"
     return Vector3(x=input[0], y=input[1], z=input[2])
 
 
-def pose_numpy_to_msg(pose: np_array7) -> Pose:
+def pose_numpy_to_msg(input: np_array7) -> Pose:
     """Converts Numpy array of shape (7,) to ROS Pose message.
     Expected order of axes is (position.x, position.y, position.z, orientation.x,
     orientation.y, orientation.z, orientation.w).
 
     Args:
-        pose (npt.NDArray[np.float64], Literal[7]): Input pose as Numpy array.
+        input (npt.NDArray[np.float64], Literal[7]): Input pose as Numpy array.
 
     Returns:
         geometry_msgs.msg.Pose: Pose represented as a ROS message.
     """
-    assert len(pose.shape) == 7, "Input pose is not of a length 7."
-    return Wrench(
-        position=Point(x=pose[0], y=pose[1], z=pose[2]),
-        orientation=Quaternion(x=pose[3], y=pose[4], z=pose[5], w=pose[6]),
+    assert (
+        input.ndim == 1
+    ), f"Input vector has '{input.ndim}' dimensions, expected dimension size of 1"
+    assert (
+        input.size == 7
+    ), f"Input vector has length of '{input.size}', expected length '7'!"
+    return Pose(
+        position=Point(x=input[0], y=input[1], z=input[2]),
+        orientation=Quaternion(x=input[3], y=input[4], z=input[5], w=input[6]),
     )
 
 
-def wrench_numpy_to_msg(wrench: np_array6) -> Wrench:
+def wrench_numpy_to_msg(input: np_array6) -> Wrench:
     """Converts Numpy array of shape (6,) to ROS Wrench message.
     Expected order of axes is (force.x, force.y, force.z, torque.x,
     torque.y, torque.z).
 
     Args:
-        wrench (npt.NDArray[np.float64], Literal[6]): Input wrench as Numpy array.
+        input (npt.NDArray[np.float64], Literal[6]): Input wrench as Numpy array.
 
     Returns:
         geometry_msgs.msg.Wrench: Wrench represented as a ROS message.
     """
-    assert len(wrench.shape) == 6, "Input wrench is not of a length 6."
+    assert (
+        input.ndim == 1
+    ), f"Input vector has '{input.ndim}' dimensions, expected dimension size of 1"
+    assert (
+        input.size == 6
+    ), f"Input vector has length of '{input.size}', expected length '6'!"
     return Wrench(
-        force=vector3_numpy_to_msg(wrench[:3]),
-        torque=vector3_numpy_to_msg(wrench[3:]),
+        force=vector3_numpy_to_msg(input[:3]),
+        torque=vector3_numpy_to_msg(input[3:]),
     )
 
 
-def twist_numpy_to_msg(twist: np_array6) -> Twist:
+def twist_numpy_to_msg(input: np_array6) -> Twist:
     """Converts Numpy array of shape (6,) to ROS Twist message.
     Expected order of axes is (linear.x, linear.y, linear.z, angular.x,
     angular.y, angular.z).
 
     Args:
-        twist (npt.NDArray[np.float64], Literal[6]): Input twist as Numpy array.
+        input (npt.NDArray[np.float64], Literal[6]): Input twist as Numpy array.
 
     Returns:
         geometry_msgs.msg.Twist: Twist represented as a ROS message.
     """
-    assert len(twist.shape) == 6, "Input twist is not of a length 6."
+    assert (
+        input.ndim == 1
+    ), f"Input vector has '{input.ndim}' dimensions, expected dimension size of 1"
+    assert (
+        input.size == 6
+    ), f"Input vector has length of '{input.size}', expected length ''6!"
     return Twist(
-        linear=vector3_numpy_to_msg(twist[:3]),
-        angular=vector3_numpy_to_msg(twist[3:]),
+        linear=vector3_numpy_to_msg(input[:3]),
+        angular=vector3_numpy_to_msg(input[3:]),
     )
 
 
@@ -117,7 +137,8 @@ def wrench_msg_to_numpy(msg: Wrench) -> np_array6:
         msg (geometry_msgs.msg.Wrench): Input ROS Wrench message.
 
     Returns:
-        npt.NDArray[np.float64], Literal[6]: Numpy array of shape (6,) with force and torque vector concatenated.
+        npt.NDArray[np.float64], Literal[6]: Numpy array of shape (6,)
+        with force and torque vector concatenated.
     """
     return np.array(
         [
@@ -155,41 +176,43 @@ def twist_msg_to_numpy(msg: Twist) -> np_array6:
     )
 
 
-def matrix_numpy_to_msg(matrix: npt.NDArray[np.float64]) -> Float64MultiArray:
+def matrix_numpy_to_msg(input: npt.NDArray[np.float64]) -> Float64MultiArray:
     """Converts Numpy array into ROS array message.
 
     Args:
-        matrix (npt.NDArray[np.float64]]): Input matrix.
+        input (npt.NDArray[np.float64]]): Input matrix.
 
     Returns:
         Float64MultiArray: ROS message with the matrix.
     """
-    assert len(matrix.shape) == 2, "Input matrix is not 2D."
+    assert (
+        input.ndim == 2
+    ), f"Input matrix is dimension '{input.ndim}'. EXpected 2D matrix!"
 
     m = Float64MultiArray()
     m.layout.data_offset = 0
     m.layout.dim = [MultiArrayDimension(), MultiArrayDimension()]
     m.layout.dim[0].label = "rows"
-    m.layout.dim[0].size = matrix.shape[0]
-    m.layout.dim[0].stride = matrix.size
+    m.layout.dim[0].size = input.shape[0]
+    m.layout.dim[0].stride = input.size
     m.layout.dim[1].label = "cols"
-    m.layout.dim[1].stride = matrix.shape[1]
-    m.layout.dim[1].size = matrix.shape[1]
+    m.layout.dim[1].stride = input.shape[1]
+    m.layout.dim[1].size = input.shape[1]
     # Flatten the matrix to a vector
-    m.data = matrix.reshape(-1).tolist()
+    m.data = input.reshape(-1).tolist()
     return m
 
 
 def matrix_msg_to_numpy(msg: Float64MultiArray) -> npt.NDArray[np.float64]:
-    """Converts ROS array message into numpy array of a shape [nx, nu] with Riccati gains.
+    """Converts ROS array message into numpy array.
 
     Args:
-        msg (Float64MultiArray): Input ROS message with array of Riccati gains.
+        msg (Float64MultiArray): Input ROS message with array..
 
     Returns:
-        npt.NDArray[np.float64]: Output numpy matrix with Riccati gains.
+        npt.NDArray[np.float64]: Output numpy matrix.
     """
-    assert msg.layout.dim.size() == 2, "The ROS message must be a 2D matrix."
+    assert len(msg.layout.dim) == 2, "The ROS message must be a 2D matrix."
     return np.array(msg.data).reshape(msg.layout.dim[0].size, msg.layout.dim[1].size)
 
 

tests/test_numpy_conversions.py

@@ -0,0 +1,192 @@
+#!/usr/bin/env python
+
+import numpy as np
+import numpy.typing as npt
+from typing import List
+from copy import deepcopy
+
+from geometry_msgs.msg import Transform, Vector3, Quaternion
+
+from linear_feedback_controller_msgs_py.numpy_conversions import *
+import linear_feedback_controller_msgs_py.lfc_py_types as lfc_py_types
+
+
+def test_check_numpy_constructors() -> None:
+    joint_state = lfc_py_types.JointState(
+        name=["joint_0", "joint_1"],
+        position=np.array([1.0, 1.0]),
+        velocity=np.array([1.0, 1.0]),
+        effort=np.array([1.0, 1.0]),
+    )
+    contact = lfc_py_types.Contact(
+        active=False,
+        name="contact_0",
+        wrench=np.ones(6),
+        pose=np.ones(7),
+    )
+    sensor = lfc_py_types.Sensor(
+        base_pose=np.ones(7),
+        base_twist=np.ones(6),
+        joint_state=joint_state,
+        contacts=[contact],
+    )
+    lfc_py_types.Control(
+        feedback_gain=np.ones((4, 4)),
+        feedforward=np.ones((4, 4)),
+        initial_state=sensor,
+    )
+
+
+def test_check_ros_numpy_matrix_conversion() -> None:
+    numpy_random_matrix = np.random.rand(5, 6)
+
+    ros_matrix = matrix_numpy_to_msg(deepcopy(numpy_random_matrix))
+    numpy_back_converted_matrix = matrix_msg_to_numpy(ros_matrix)
+
+    np.testing.assert_array_equal(
+        numpy_back_converted_matrix,
+        numpy_random_matrix,
+        err_msg="Matrix after conversion back to "
+        + "Numpy is not equal the initial matrix!",
+    )
+
+
+def test_check_ros_numpy_joint_state_conversion() -> None:
+    numpy_joint_state = lfc_py_types.JointState(
+        name=["1", "2", "3", "4", "5", "6"],
+        position=np.random.rand(6),
+        velocity=np.random.rand(6),
+        effort=np.random.rand(6),
+    )
+
+    # Pass deep copy to ensure memory is separate
+    ros_joint_state = joint_state_numpy_to_msg(deepcopy(numpy_joint_state))
+    numpy_back_converted_join_state = joint_state_msg_to_numpy(ros_joint_state)
+
+    assert (
+        numpy_joint_state.name == numpy_back_converted_join_state.name
+    ), "Names after conversion back to Numpy is not equal initial values!"
+
+    np.testing.assert_array_equal(
+        numpy_joint_state.position,
+        numpy_back_converted_join_state.position,
+        err_msg="Positions after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_joint_state.velocity,
+        numpy_back_converted_join_state.velocity,
+        err_msg="Velocities after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_joint_state.effort,
+        numpy_back_converted_join_state.effort,
+        err_msg="Efforts after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+
+def test_check_ros_eigen_sensor_conversions() -> None:
+    quat = np.random.rand(4)
+    quat = quat / np.linalg.norm(quat)
+
+    numpy_sensor = lfc_py_types.Sensor(
+        base_pose=np.concatenate((np.random.rand(3), quat)),
+        base_twist=np.random.rand(6),
+        joint_state=lfc_py_types.JointState(
+            name=["1", "2", "3", "4", "5", "6"],
+            position=np.random.rand(6),
+            velocity=np.random.rand(6),
+            effort=np.random.rand(6),
+        ),
+        contacts=[],
+    )
+
+    numpy_sensor.contacts.append(
+        lfc_py_types.Contact(
+            active=True,
+            name="left_foot",
+            wrench=np.random.rand(6),
+            pose=np.random.rand(7),
+        )
+    )
+
+    numpy_sensor.contacts.append(
+        lfc_py_types.Contact(
+            active=False,
+            name="right_foot",
+            wrench=np.random.rand(6),
+            pose=np.random.rand(7),
+        )
+    )
+
+    ros_sensor = sensor_numpy_to_msg(deepcopy(numpy_sensor))
+    numpy_back_converted_sensor = sensor_msg_to_numpy(ros_sensor)
+
+    np.testing.assert_array_equal(
+        numpy_sensor.base_pose,
+        numpy_back_converted_sensor.base_pose,
+        err_msg="Base Pose after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_sensor.base_twist,
+        numpy_back_converted_sensor.base_twist,
+        err_msg="Base Twist after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    assert (
+        numpy_sensor.joint_state.name == numpy_back_converted_sensor.joint_state.name
+    ), "Joint names after conversion back to Numpy is not equal initial values!"
+
+    np.testing.assert_array_equal(
+        numpy_sensor.joint_state.position,
+        numpy_back_converted_sensor.joint_state.position,
+        err_msg="Joint positions after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_sensor.joint_state.velocity,
+        numpy_back_converted_sensor.joint_state.velocity,
+        err_msg="Joint velocities after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_sensor.joint_state.effort,
+        numpy_back_converted_sensor.joint_state.effort,
+        err_msg="Joint efforts after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    assert len(numpy_sensor.contacts) == len(
+        numpy_back_converted_sensor.contacts
+    ), "Number of contacts after conversion doesn't match number of contacts before conversion!"
+
+    for i in range(len(numpy_sensor.contacts)):
+        c1 = numpy_sensor.contacts[i]
+        c2 = numpy_back_converted_sensor.contacts[i]
+        assert (
+            c1.active == c2.active
+        ), f"Active parameter before and after conversion differs at index '{i}'"
+        assert (
+            c1.name == c2.name
+        ), f"Name parameter before and after conversion differs at index '{i}'"
+
+        np.testing.assert_array_equal(
+            c1.wrench,
+            c2.wrench,
+            err_msg=f"Wrench parameter before and after conversion differs at index '{i}'",
+        )
+
+        np.testing.assert_array_equal(
+            c1.pose,
+            c2.pose,
+            err_msg=f"Pose parameter before and after conversion differs at index '{i}'",
+        )