Add missing control Numpy conversions test

linear_feedback_controller_msgs_py/numpy_conversions.py

@@ -180,39 +180,47 @@ def matrix_numpy_to_msg(input: npt.NDArray[np.float64]) -> Float64MultiArray:
     """Converts Numpy array into ROS array message.
 
     Args:
-        input (npt.NDArray[np.float64]]): Input matrix.
+        input (npt.NDArray[np.float64]]): Input matrix of size (N,2) or (N,).
 
     Returns:
         std_msgs.msg.Float64MultiArray: ROS message with the matrix.
     """
+    # In case vector is passed consider is (N,1) array.
+    rows, cols = input.shape if input.ndim != 1 else (input.shape[0], 1)
     assert (
-        input.ndim == 2
-    ), f"Input matrix is dimension '{input.ndim}'. EXpected 2D matrix!"
+        input.ndim == 2 or input.ndim == 1
+    ), f"Input matrix is dimension '{input.ndim}'. Expected 2D matrix or 1D vector!"
 
     m = Float64MultiArray()
     m.layout.data_offset = 0
     m.layout.dim = [MultiArrayDimension(), MultiArrayDimension()]
     m.layout.dim[0].label = "rows"
-    m.layout.dim[0].size = input.shape[0]
+    m.layout.dim[0].size = rows
     m.layout.dim[0].stride = input.size
     m.layout.dim[1].label = "cols"
-    m.layout.dim[1].stride = input.shape[1]
-    m.layout.dim[1].size = input.shape[1]
+    m.layout.dim[1].stride = cols
+    m.layout.dim[1].size = cols
     # Flatten the matrix to a vector
     m.data = input.reshape(-1).tolist()
     return m
 
 
-def matrix_msg_to_numpy(msg: Float64MultiArray) -> npt.NDArray[np.float64]:
+def matrix_msg_to_numpy(
+    msg: Float64MultiArray, return_vector: bool = True
+) -> npt.NDArray[np.float64]:
     """Converts ROS array message into numpy array.
 
     Args:
         msg (std_msgs.msg.Float64MultiArray): Input ROS message with array.
+        return_vector (bool, optional): If ``True`` vector is returned in a shape (N,)
+        otherwise the shape is (N,1). Defaults to True.
 
     Returns:
         npt.NDArray[np.float64]: Output numpy matrix.
     """
-    assert len(msg.layout.dim) == 2, "The ROS message must be a 2D matrix."
+    assert len(msg.layout.dim) == 2, "The ROS message must be a 2D matrix!"
+    if return_vector and msg.layout.dim[1].size == 1:
+        return np.array(msg.data)
     return np.array(msg.data).reshape(msg.layout.dim[0].size, msg.layout.dim[1].size)
 
 

tests/test_numpy_conversions.py

@@ -1,12 +1,8 @@
 #!/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
 
@@ -50,6 +46,25 @@ def test_check_ros_numpy_matrix_conversion() -> None:
         + "Numpy is not equal the initial matrix!",
     )
 
+    numpy_random_vector = np.random.rand(20)
+
+    ros_vector = matrix_numpy_to_msg(deepcopy(numpy_random_vector))
+
+    for return_vector in (True, False):
+        numpy_back_converted_vector = matrix_msg_to_numpy(ros_vector, return_vector)
+
+        np.testing.assert_array_equal(
+            numpy_back_converted_vector,
+            (
+                numpy_random_vector
+                if return_vector
+                else numpy_random_vector.reshape(-1, 1)
+            ),
+            err_msg="Vector after conversion back to "
+            + "Numpy is not equal the initial vector "
+            + f"for case with 'return_vector={return_vector}'",
+        )
+
 
 def test_check_ros_numpy_joint_state_conversion() -> None:
     numpy_joint_state = lfc_py_types.JointState(
@@ -89,6 +104,84 @@ def test_check_ros_numpy_joint_state_conversion() -> None:
     )
 
 
+def test_check_ros_eigen_control_conversion() -> None:
+    quat = np.random.rand(4)
+    quat = quat / np.linalg.norm(quat)
+
+    numpy_control = lfc_py_types.Control(
+        initial_state=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=[],
+        ),
+        feedback_gain=np.random.rand(8, 4),
+        feedforward=np.random.rand(4),
+    )
+
+    ros_control_msg = control_numpy_to_msg(deepcopy(numpy_control))
+    back_converted_numpy_control = control_msg_to_numpy(ros_control_msg)
+
+    np.testing.assert_array_equal(
+        numpy_control.initial_state.base_pose,
+        back_converted_numpy_control.initial_state.base_pose,
+        err_msg="Initial state base pose after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_control.initial_state.base_twist,
+        back_converted_numpy_control.initial_state.base_twist,
+        err_msg="Initial state base twist after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    assert (
+        numpy_control.initial_state.joint_state.name
+        == back_converted_numpy_control.initial_state.joint_state.name
+    ), "Names after conversion back to Numpy is not equal initial values!"
+
+    np.testing.assert_array_equal(
+        numpy_control.initial_state.joint_state.position,
+        back_converted_numpy_control.initial_state.joint_state.position,
+        err_msg="Joint state position after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_control.initial_state.joint_state.velocity,
+        back_converted_numpy_control.initial_state.joint_state.velocity,
+        err_msg="Joint state velocity after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_control.initial_state.joint_state.effort,
+        back_converted_numpy_control.initial_state.joint_state.effort,
+        err_msg="Joint state effort after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_control.feedback_gain,
+        back_converted_numpy_control.feedback_gain,
+        err_msg="Feedback gains after conversion back to "
+        + "Numpy is not equal initial values!",
+    )
+
+    np.testing.assert_array_equal(
+        numpy_control.feedforward,
+        back_converted_numpy_control.feedforward,
+        err_msg="Feed forward 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)