Skip to content

Commit

Permalink
[ROS2] transformstamped (#27)
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatthijsBurgh
MatthijsBurgh authored Nov 23, 2023
2 parents 4434e35 + 38ed099 commit f65a2a5
Show file tree
Hide file tree
Showing 2 changed files with 201 additions and 33 deletions.
128 changes: 109 additions & 19 deletions tf2_pykdl_ros/test/test_tf2_ros_integration.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@ def test_vector_stamped_to_msg(self) -> None:
def test_vector_stamped_convert(self) -> None:
self.convert_reg.get_convert((PointStamped, VectorStamped))
self.convert_reg.get_convert((PoseStamped, VectorStamped))
self.convert_reg.get_convert((TransformStamped, VectorStamped))

self.convert_reg.get_convert((VectorStamped, PointStamped))
self.convert_reg.get_convert((VectorStamped, VectorStamped))

Expand All @@ -48,8 +50,13 @@ def test_frame_stamped_to_msg(self) -> None:
self.convert_reg.get_to_msg(FrameStamped)

def test_frame_stamped_convert(self) -> None:
self.convert_reg.get_convert((PointStamped, FrameStamped))
self.convert_reg.get_convert((PoseStamped, FrameStamped))
self.convert_reg.get_convert((TransformStamped, FrameStamped))

self.convert_reg.get_convert((FrameStamped, PointStamped))
self.convert_reg.get_convert((FrameStamped, PoseStamped))
self.convert_reg.get_convert((FrameStamped, TransformStamped))
self.convert_reg.get_convert((FrameStamped, FrameStamped))

def test_frame_stamped_transform(self) -> None:
Expand Down Expand Up @@ -85,13 +92,36 @@ def test_wrench_stamped_transform(self) -> None:


class TestConvert(unittest.TestCase):
point_stamped: ClassVar[PointStamped]
pose_stamped: ClassVar[PoseStamped]
frame_stamped: ClassVar[FrameStamped]

@classmethod
def setUpClass(cls) -> None:
point = PointStamped()
point.header.frame_id = "map"
point.header.stamp = Time(sec=4)
point.point.x = 1.0
point.point.y = 2.0
point.point.z = 3.0
cls.point_stamped = point

pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = Time(sec=4)
pose.pose.position.x = 1.0
pose.pose.position.y = 2.0
pose.pose.position.z = 3.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
cls.pose_stamped = pose

cls.frame_stamped = FrameStamped.from_xyz_rpy(1, 2, 3, 4, 5, 6, Time(sec=4), "map")

def test_point_stamped_vector_stamped(self) -> None:
p = PointStamped()
p.header.frame_id = "map"
p.header.stamp = Time(sec=4)
p.point.x = 1.0
p.point.y = 2.0
p.point.z = 3.0
p = self.point_stamped
v = tf2_ros.convert(p, VectorStamped)
self.assertIsInstance(v, VectorStamped)
self.assertEqual(v.vector.x(), p.point.x)
Expand All @@ -100,6 +130,16 @@ def test_point_stamped_vector_stamped(self) -> None:
self.assertEqual(v.header.stamp, p.header.stamp)
self.assertEqual(v.header.frame_id, p.header.frame_id)

def test_pose_stamped_vector_stamped(self) -> None:
p = self.pose_stamped
v = tf2_ros.convert(p, VectorStamped)
self.assertIsInstance(v, VectorStamped)
self.assertEqual(v.vector.x(), p.pose.position.x)
self.assertEqual(v.vector.y(), p.pose.position.y)
self.assertEqual(v.vector.z(), p.pose.position.z)
self.assertEqual(v.header.stamp, p.header.stamp)
self.assertEqual(v.header.frame_id, p.header.frame_id)

def test_vector_stamped_point_stamped(self) -> None:
v = VectorStamped.from_xyz(1, 2, 3, Time(sec=4), "map")
p = tf2_ros.convert(v, PointStamped)
Expand All @@ -110,28 +150,62 @@ def test_vector_stamped_point_stamped(self) -> None:
self.assertEqual(p.header.stamp, v.header.stamp)
self.assertEqual(p.header.frame_id, v.header.frame_id)

def test_pose_stamp_frame_stamped(self) -> None:
p = PoseStamped()
p.header.frame_id = "map"
p.header.stamp = Time(sec=4)
p.pose.position.x = 1.0
p.pose.position.y = 2.0
p.pose.position.z = 3.0
p.pose.orientation.x = 0.0
p.pose.orientation.y = 0.0
p.pose.orientation.z = 0.0
p.pose.orientation.w = 1.0
def test_point_stamped_frane_stamped(self) -> None:
p = self.point_stamped
f = tf2_ros.convert(p, FrameStamped)
self.assertIsInstance(f, FrameStamped)
self.assertEqual(f.frame.p.x(), p.point.x)
self.assertEqual(f.frame.p.y(), p.point.y)
self.assertEqual(f.frame.p.z(), p.point.z)
self.assertEqual(f.frame.M.GetQuaternion(), kdl.Rotation.Identity().GetQuaternion())
self.assertEqual(f.header.stamp, p.header.stamp)
self.assertEqual(f.header.frame_id, p.header.frame_id)

def test_pose_stamped_frame_stamped(self) -> None:
p = self.pose_stamped
f = tf2_ros.convert(p, FrameStamped)
self.assertIsInstance(f, FrameStamped)
self.assertEqual(f.frame.p.x(), p.pose.position.x)
self.assertEqual(f.frame.p.y(), p.pose.position.y)
self.assertEqual(f.frame.p.z(), p.pose.position.z)
self.assertEqual(f.frame.M.GetQuaternion(), (0.0, 0.0, 0.0, 1.0))
q = p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w
self.assertEqual(f.frame.M.GetQuaternion(), q)
self.assertEqual(f.header.stamp, p.header.stamp)
self.assertEqual(f.header.frame_id, p.header.frame_id)

def test_transform_stamped_frame_stamped(self) -> None:
t = TransformStamped()
t.transform.translation.x = self.pose_stamped.pose.position.x
t.transform.translation.y = self.pose_stamped.pose.position.y
t.transform.translation.z = self.pose_stamped.pose.position.z
t.transform.rotation.x = self.pose_stamped.pose.orientation.x
t.transform.rotation.y = self.pose_stamped.pose.orientation.y
t.transform.rotation.z = self.pose_stamped.pose.orientation.z
t.transform.rotation.w = self.pose_stamped.pose.orientation.w
t.header = self.pose_stamped.header
t.child_frame_id = "child" # Not used
f = tf2_ros.convert(t, FrameStamped)
self.assertIsInstance(f, FrameStamped)
self.assertEqual(f.frame.p.x(), t.transform.translation.x)
self.assertEqual(f.frame.p.y(), t.transform.translation.y)
self.assertEqual(f.frame.p.z(), t.transform.translation.z)
q = t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w
self.assertEqual(f.frame.M.GetQuaternion(), q)
self.assertEqual(f.header.stamp, t.header.stamp)
self.assertEqual(f.header.frame_id, t.header.frame_id)

def test_frame_stamped_point_stamped(self) -> None:
f = self.frame_stamped
p = tf2_ros.convert(f, PointStamped)
self.assertIsInstance(p, PointStamped)
self.assertEqual(p.point.x, f.frame.p.x())
self.assertEqual(p.point.y, f.frame.p.y())
self.assertEqual(p.point.z, f.frame.p.z())
self.assertEqual(p.header.stamp, f.header.stamp)
self.assertEqual(p.header.frame_id, f.header.frame_id)

def test_frame_stamped_pose_stamped(self) -> None:
f = FrameStamped.from_xyz_rpy(1, 2, 3, 4, 5, 6, Time(sec=4), "map")
f = self.frame_stamped
p = tf2_ros.convert(f, PoseStamped)
self.assertIsInstance(p, PoseStamped)
self.assertEqual(p.pose.position.x, f.frame.p.x())
Expand All @@ -145,6 +219,22 @@ def test_frame_stamped_pose_stamped(self) -> None:
self.assertEqual(p.header.stamp, f.header.stamp)
self.assertEqual(p.header.frame_id, f.header.frame_id)

def test_frame_stamped_transform_stamped(self) -> None:
f = self.frame_stamped
t = tf2_ros.convert(f, TransformStamped)
self.assertIsInstance(t, TransformStamped)
self.assertEqual(t.transform.translation.x, f.frame.p.x())
self.assertEqual(t.transform.translation.y, f.frame.p.y())
self.assertEqual(t.transform.translation.z, f.frame.p.z())
q = f.frame.M.GetQuaternion()
self.assertEqual(t.transform.rotation.x, q[0])
self.assertEqual(t.transform.rotation.y, q[1])
self.assertEqual(t.transform.rotation.z, q[2])
self.assertEqual(t.transform.rotation.w, q[3])
self.assertEqual(t.header.stamp, f.header.stamp)
self.assertEqual(t.header.frame_id, f.header.frame_id)
self.assertEqual(t.child_frame_id, "") # Not set

def test_twist_stamped_msg_twist_stamped(self) -> None:
msg = TwistStampedMsg()
msg.header.frame_id = "map"
Expand Down
106 changes: 92 additions & 14 deletions tf2_pykdl_ros/tf2_pykdl_ros/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,24 +46,27 @@ def to_msg_vector(vector: VectorStamped) -> PointStamped:
tf2_ros.ConvertRegistration().add_to_msg(VectorStamped, to_msg_vector)


def from_msg_vector(msg: Union[PointStamped, PoseStamped]) -> VectorStamped:
def from_msg_vector(msg: Union[PointStamped, PoseStamped, TransformStamped]) -> VectorStamped:
"""
Convert a PointStamped/PoseStamped message to a stamped VectorStamped.
Convert a PointStamped/PoseStamped/TransformStamped message to a stamped VectorStamped.
:param msg: The PointStamped/PoseStamped message to convert.
:param msg: The PointStamped/PoseStamped/TransformStamped message to convert.
:return: The timestamped converted PyKDL vector.
"""
if isinstance(msg, PointStamped):
v = msg.point
elif isinstance(msg, PoseStamped):
v = msg.pose.position
elif isinstance(msg, TransformStamped):
v = msg.transform.translation
else:
raise TypeError(f"msg should be PointStamped or PoseStamped, not '{type(msg)}'")
raise TypeError(f"msg should be PointStamped, PoseStamped or TransformStamped, not '{type(msg)}'")
return VectorStamped.from_xyz(v.x, v.y, v.z, msg.header.stamp, msg.header.frame_id)


tf2_ros.ConvertRegistration().add_convert((PointStamped, VectorStamped), from_msg_vector)
tf2_ros.ConvertRegistration().add_convert((PoseStamped, VectorStamped), from_msg_vector)
tf2_ros.ConvertRegistration().add_convert((TransformStamped, VectorStamped), from_msg_vector)
tf2_ros.ConvertRegistration().add_from_msg(VectorStamped, from_msg_vector)


Expand Down Expand Up @@ -96,7 +99,26 @@ def do_transform_vector(vector: VectorStamped, transform: TransformStamped) -> V
tf2_ros.TransformRegistration().add(VectorStamped, do_transform_vector)


def to_msg_frame(frame: FrameStamped) -> PoseStamped:
def to_point_msg_frame(frame: FrameStamped) -> PointStamped:
"""
Convert a FrameStamped to a geometry_msgs PointStamped message.
:param frame: The frame to convert.
:return: The converted point.
"""
msg = PointStamped()
msg.header = frame.header
v = frame.frame.p
msg.point.x = v[0]
msg.point.y = v[1]
msg.point.z = v[2]
return msg


tf2_ros.ConvertRegistration().add_convert((FrameStamped, PointStamped), to_point_msg_frame)


def to_pose_msg_frame(frame: FrameStamped) -> PoseStamped:
"""
Convert a FrameStamped to a geometry_msgs PoseStamped message.
Expand All @@ -117,28 +139,84 @@ def to_msg_frame(frame: FrameStamped) -> PoseStamped:
return msg


tf2_ros.ConvertRegistration().add_convert((FrameStamped, PoseStamped), to_msg_frame)
tf2_ros.ConvertRegistration().add_to_msg(FrameStamped, to_msg_frame)
tf2_ros.ConvertRegistration().add_convert((FrameStamped, PoseStamped), to_pose_msg_frame)
tf2_ros.ConvertRegistration().add_to_msg(FrameStamped, to_pose_msg_frame)


def to_transform_msg_frame(frame: FrameStamped) -> TransformStamped:
"""
Convert a FrameStamped to a geometry_msgs TransformStamped message.
The child_frame_id is not set.
:param frame: The frame to convert.
:return: The converted Pose.
"""
msg = TransformStamped()
msg.header = frame.header
v = frame.frame.p
msg.transform.translation.x = v[0]
msg.transform.translation.y = v[1]
msg.transform.translation.z = v[2]
q = frame.frame.M.GetQuaternion()
msg.transform.rotation.x = q[0]
msg.transform.rotation.y = q[1]
msg.transform.rotation.z = q[2]
msg.transform.rotation.w = q[3]
return msg


tf2_ros.ConvertRegistration().add_convert((FrameStamped, TransformStamped), to_transform_msg_frame)


def from_msg_frame(msg: PoseStamped) -> FrameStamped:
def from_point_msg_frame(msg: PointStamped) -> FrameStamped:
"""
Convert a PoseStamped message to a stamped FrameStamped.
Convert a PointStamped message to a stamped FrameStamped.
:param msg: The PoseStamped message to convert.
:param msg: The PointStamped message to convert.
:return: The timestamped converted PyKDL frame.
"""
if not isinstance(msg, PoseStamped):
raise TypeError(f"msg should be PoseStamped, not '{type(msg)}'")
v = msg.pose.position
if not isinstance(msg, PointStamped):
raise TypeError(f"msg should be a PointStamped, not '{type(msg)}'")
vector = kdl.Vector(msg.point.x, msg.point.y, msg.point.z)
frame = kdl.Frame(vector)
return FrameStamped(frame, msg.header.stamp, msg.header.frame_id)


tf2_ros.ConvertRegistration().add_convert((PointStamped, FrameStamped), from_point_msg_frame)


def from_msg_frame(msg: Union[PoseStamped, TransformStamped]) -> FrameStamped:
"""
Convert a PoseStamped or TransformStamped message to a stamped FrameStamped.
:param msg: The message to convert.
:return: The timestamped converted PyKDL frame.
"""
if not isinstance(msg, PoseStamped) and not isinstance(msg, TransformStamped):
raise TypeError(f"msg should be a PoseStamped or TransformStamped, not '{type(msg)}'")

if isinstance(msg, PoseStamped):
pose_attr = "pose"
position_attr = "position"
orientation_attr = "orientation"
elif isinstance(msg, TransformStamped):
pose_attr = "transform"
position_attr = "translation"
orientation_attr = "rotation"

pose = getattr(msg, pose_attr)
v = getattr(pose, position_attr)
vector = kdl.Vector(v.x, v.y, v.z)
q = msg.pose.orientation
q = getattr(pose, orientation_attr)
rotation = kdl.Rotation.Quaternion(q.x, q.y, q.z, q.w)

frame = kdl.Frame(rotation, vector)
return FrameStamped(frame, msg.header.stamp, msg.header.frame_id)


tf2_ros.ConvertRegistration().add_convert((PoseStamped, FrameStamped), from_msg_frame)
tf2_ros.ConvertRegistration().add_convert((TransformStamped, FrameStamped), from_msg_frame)
tf2_ros.ConvertRegistration().add_from_msg(FrameStamped, from_msg_frame)


Expand Down

0 comments on commit f65a2a5

Please sign in to comment.