def __init__(
self,
frame,
base=None,
topic=None,
max_points=1000,
publish_interval=0.0,
scale=0.1,
color="#ffffffff",
verbose=False,
):
""" Constructor.
Parameters
----------
frame : str or ReferenceFrame
Reference frame for which to publish the translation.
base : str or ReferenceFrame, optional
Base reference wrt to which the translation is published. Defaults
to the parent reference frame.
topic : str, optional
Name of the topic on which to publish. Defaults to "/<frame>/path".
max_points : int, default 1000
Maximum number of points to add to the marker. Actual translation
array will be sub-sampled to this number of points.
publish_interval : float, default 0.0
Time in seconds between publishing when calling ``spin``.
"""
from geometry_msgs.msg import Point
self.frame = _resolve_rf(frame)
self.base = _resolve_rf(base or self.frame.parent)
self.translation, _, _ = self.frame.lookup_transform(self.base)
marker = get_marker(frame_id=self.base.name,
scale=(scale, 0.0, 0.0),
color=color)
show_every = self.translation.shape[0] // max_points
marker.points = [Point(*row) for row in self.translation[::show_every]]
topic = topic or f"/{self.frame.name}/path"
super().__init__(marker,
topic,
publish_interval=publish_interval,
verbose=verbose)
def render_tree(root):
""" Render a reference frame tree.
Parameters
----------
root: str or ReferenceFrame
The root of the rendered tree.
"""
for pre, _, node in RenderTree(_resolve_rf(root)):
print(f"{pre}{node.name}")
def plot_reference_frame(
frame, world_frame=None, ax=None, figsize=(6, 6), arrow_len=1.0
):
""" Plot a 3D coordinate system representing a static reference frame.
Parameters
----------
frame: str or ReferenceFrame
The reference frame to plot. If str, the frame will be looked up
in the registry under that name.
world_frame: str or ReferenceFrame, optional
If specified, the world reference frame that defines the origin of the
plot. If str, the frame will be looked up in the registry under that
name.
ax: matplotlib.axes.Axes instance, optional
If provided, plot the points onto these axes.
figsize:
If `ax` is not provided, create a figure of this size.
arrow_len:
Length of the arrows of the coordinate system.
Returns
-------
arrows: list of Arrow3D
A list of three arrows representing the plotted coordinate system.
"""
if ax is None:
fig = plt.figure(figsize=figsize)
ax = fig.add_subplot(111, projection="3d")
frame = _resolve_rf(frame)
if frame.timestamps is not None:
raise NotImplementedError("Can only plot static reference frames")
arrows = _add_frame(ax, frame, world_frame, arrow_len=arrow_len)
_set_axes_equal(ax)
return arrows
def __init__(
self,
frame,
base=None,
publish_pose=False,
publish_twist=False,
subscribe=False,
twist_cutoff=None,
twist_outlier_thresh=None,
):
""" Constructor.
Parameters
----------
frame : str or ReferenceFrame
Reference frame for which to publish the transform.
base : str or ReferenceFrame, optional
Base reference wrt to which the transform is published. Defaults
to the parent reference frame.
publish_pose : bool, default False
If True, also publish a PoseStamped message on the topic
"/<frame>/pose".
publish_twist : bool, default False
If True, also publish a TwistStamped message with the linear and
angular velocity of the frame wrt the base on the topic
"/<frame>/twist".
subscribe : bool or str, default False
If True, subscribe to the "/tf" topic and publish transforms
when messages are broadcast whose `child_frame_id` is the name of
the base frame. If the frame is a moving reference frame, the
transform whose timestamp is the closest to the broadcast timestamp
is published. `subscribe` can also be a string, in which case this
broadcaster will be listening for TF messages with this
`child_frame_id`.
"""
import pandas as pd
import rospy
import tf2_ros
from geometry_msgs.msg import PoseStamped, TwistStamped
from tf.msg import tfMessage
self.frame = _resolve_rf(frame)
self.base = _resolve_rf(base or self.frame.parent)
(
self.translation,
self.rotation,
self.timestamps,
) = self.frame.lookup_transform(self.base)
if self.timestamps is None:
self.broadcaster = tf2_ros.StaticTransformBroadcaster()
else:
self.timestamps = pd.Index(self.timestamps)
self.broadcaster = tf2_ros.TransformBroadcaster()
if publish_pose:
self.pose_publisher = rospy.Publisher(
f"/{self.frame.name}/pose",
PoseStamped,
queue_size=1,
latch=True,
)
else:
self.pose_publisher = None
if publish_twist:
self.linear, self.angular = self.frame.lookup_twist(
reference=base,
represent_in=self.frame,
cutoff=twist_cutoff,
outlier_thresh=twist_outlier_thresh,
)
self.twist_publisher = rospy.Publisher(
f"/{self.frame.name}/twist",
TwistStamped,
queue_size=1,
latch=True,
)
else:
self.twist_publisher = None
if subscribe:
self.subscriber = rospy.Subscriber("/tf", tfMessage,
self.handle_incoming_msg)
if isinstance(subscribe, str):
self.subscribe_to_frame = subscribe
else:
self.subscribe_to_frame = self.base.name
else:
self.subscriber = None
self.idx = 0
self.stopped = False
self._thread = None
def transform_linear_velocity(
self,
arr,
to_frame,
what="reference_frame",
moving_frame=None,
reference_frame=None,
axis=-1,
time_axis=0,
timestamps=None,
return_timestamps=False,
outlier_thresh=None,
cutoff=None,
):
""" Transform array of linear velocities from this frame to another.
Parameters
----------
arr: array_like
The array to transform.
to_frame: str or ReferenceFrame
The target reference frame. If str, the frame will be looked up
in the registry under that name.
what: str, default "reference_frame"
What frame of the velocity to transform. Can be "reference_frame",
"moving_frame" or "representation_frame".
moving_frame: str or ReferenceFrame, optional
The moving frame when transforming the reference frame of the
velocity.
reference_frame: str or ReferenceFrame, optional
The reference frame when transforming the moving frame of the
velocity.
axis: int, default -1
The axis of the array representing the spatial coordinates of the
velocities.
time_axis: int, default 0
The axis of the array representing the timestamps of the
velocities.
timestamps: array_like, optional
The timestamps of the velocities, corresponding to the `time_axis`
of the array. If not None, the axis defined by `time_axis` will be
re-sampled to the timestamps for which the transformation is
defined.
return_timestamps: bool, default False
If True, also return the timestamps after the transformation.
cutoff: float, optional
Frequency of a low-pass filter applied to linear and angular
velocity after the twist estimation as a fraction of the Nyquist
frequency.
outlier_thresh: float, optional
Suppress outliers by throwing out samples where the
norm of the second-order differences of the position is above
`outlier_thresh` and interpolating the missing values.
Returns
-------
arr_transformed: array_like
The transformed array.
ts: array_like, shape (n_timestamps,) or None
The timestamps after the transformation.
See Also
--------
transform_linear_velocity
"""
if what == "reference_frame":
linear, angular, linear_ts = self.lookup_twist(
to_frame,
to_frame,
cutoff=cutoff,
outlier_thresh=outlier_thresh,
allow_static=True,
return_timestamps=True,
)
angular_ts = linear_ts
translation, _, translation_ts = _resolve_rf(
moving_frame).lookup_transform(self)
elif what == "moving_frame":
to_frame = _resolve_rf(to_frame)
linear, linear_ts = to_frame.lookup_linear_velocity(
self,
to_frame,
cutoff=cutoff,
outlier_thresh=outlier_thresh,
allow_static=True,
return_timestamps=True,
)
angular, angular_ts = self.lookup_angular_velocity(
reference_frame,
to_frame,
cutoff=cutoff,
allow_static=True,
return_timestamps=True,
)
translation, _, translation_ts = to_frame.lookup_transform(self)
elif what == "representation_frame":
return self.transform_vectors(
arr,
to_frame,
axis=axis,
time_axis=time_axis,
timestamps=timestamps,
return_timestamps=return_timestamps,
)
else:
raise ValueError(
f"Expected 'what' to be 'reference_frame', 'moving_frame' or "
f"'representation_frame', got {what}")
arr, ts = self.transform_vectors(
arr,
to_frame,
axis=axis,
time_axis=time_axis,
timestamps=timestamps,
return_timestamps=True,
)
translation, translation_ts = self.transform_vectors(
translation,
to_frame,
timestamps=translation_ts,
return_timestamps=True,
)
arr, linear, angular, translation, ts_out = self._match_arrays([
(arr, ts),
(linear, linear_ts),
(angular, angular_ts),
(translation, translation_ts),
])
arr = arr + linear + np.cross(angular, translation)
if return_timestamps:
return arr, ts_out
else:
return arr
def transform_angular_velocity(
self,
arr,
to_frame,
what="reference_frame",
axis=-1,
time_axis=0,
timestamps=None,
return_timestamps=False,
cutoff=None,
):
""" Transform array of angular velocities from this frame to another.
Parameters
----------
arr: array_like
The array to transform.
to_frame: str or ReferenceFrame
The target reference frame. If str, the frame will be looked up
in the registry under that name.
what: str, default "reference_frame"
What frame of the velocity to transform. Can be "reference_frame",
"moving_frame" or "representation_frame".
axis: int, default -1
The axis of the array representing the spatial coordinates of the
velocities.
time_axis: int, default 0
The axis of the array representing the timestamps of the
velocities.
timestamps: array_like, optional
The timestamps of the velocities, corresponding to the `time_axis`
of the array. If not None, the axis defined by `time_axis` will be
re-sampled to the timestamps for which the transformation is
defined.
return_timestamps: bool, default False
If True, also return the timestamps after the transformation.
cutoff: float, optional
Frequency of a low-pass filter applied to linear and angular
velocity after the twist estimation as a fraction of the Nyquist
frequency.
Returns
-------
arr_transformed: array_like
The transformed array.
ts: array_like, shape (n_timestamps,) or None
The timestamps after the transformation.
See Also
--------
transform_angular_velocity
"""
if what == "reference_frame":
angular, angular_ts = self.lookup_angular_velocity(
to_frame,
to_frame,
cutoff=cutoff,
allow_static=True,
return_timestamps=True,
)
elif what == "moving_frame":
angular, angular_ts = _resolve_rf(
to_frame).lookup_angular_velocity(
self,
to_frame,
cutoff=cutoff,
allow_static=True,
return_timestamps=True,
)
elif what == "representation_frame":
return self.transform_vectors(
arr,
to_frame,
axis=axis,
time_axis=time_axis,
timestamps=timestamps,
return_timestamps=return_timestamps,
)
else:
raise ValueError(
f"Expected 'what' to be 'reference_frame', 'moving_frame' or "
f"'representation_frame', got {what}")
arr, ts = self.transform_vectors(
arr,
to_frame,
axis=axis,
time_axis=time_axis,
timestamps=timestamps,
return_timestamps=True,
)
arr, angular, ts_out = self._match_arrays([(arr, ts),
(angular, angular_ts)])
arr += angular
if return_timestamps:
return arr, ts_out
else:
return arr
def _walk(self, to_rf):
""" Walk from this frame to a target frame along the tree. """
to_rf = _resolve_rf(to_rf)
walker = Walker()
up, _, down = walker.walk(self, to_rf)
return up, down
def __init__(
self,
name=None,
parent=None,
translation=None,
rotation=None,
timestamps=None,
inverse=False,
discrete=False,
):
""" Constructor.
Parameters
----------
name: str, optional
The name of this reference frame.
parent: str or ReferenceFrame, optional
The parent reference frame. If str, the frame will be looked up
in the registry under that name. If not specified, this frame
will be a root node of a new reference frame tree.
translation: array_like, optional
The translation of this frame wrt the parent frame. Not
applicable if there is no parent frame.
rotation: array_like, optional
The rotation of this frame wrt the parent frame. Not
applicable if there is no parent frame.
timestamps: array_like, optional
The timestamps for translation and rotation of this frame. Not
applicable if this is a static reference frame.
inverse: bool, default False
If True, invert the transform wrt the parent frame, i.e. the
translation and rotation are specified for the parent frame wrt
this frame.
discrete: bool, default False
If True, transformations with timestamps are assumed to be events.
Instead of interpolating between timestamps, transformations are
fixed between their timestamp and the next one.
"""
super(ReferenceFrame, self).__init__()
# TODO check name requirement
self.name = name
if parent is not None:
self.parent = _resolve_rf(parent)
(
self.translation,
self.rotation,
self.timestamps,
) = self._init_arrays(translation, rotation, timestamps, inverse)
else:
self.parent = None
self._verify_root(translation, rotation, timestamps)
self.translation, self.rotation, self.timestamps = None, None, None
if discrete and self.timestamps is None:
raise ValueError("timestamps must be provided when discrete=True")
else:
self.discrete = discrete
def lookup_angular_velocity(
self,
reference=None,
represent_in=None,
outlier_thresh=None,
cutoff=None,
mode="quaternion",
allow_static=False,
return_timestamps=False,
):
""" Estimate angular velocity of this frame wrt a reference.
Parameters
----------
reference: str or ReferenceFrame, optional
The reference frame wrt which the twist is estimated. Defaults to
the parent frame.
represent_in: str or ReferenceFrame, optional
The reference frame in which the twist is represented. Defaults
to the parent frame.
outlier_thresh: float, optional
Suppress samples where the norm of the second-order differences of
the rotation is above `outlier_thresh` and interpolate the missing
values.
cutoff: float, optional
Frequency of a low-pass filter applied to linear and angular
velocity after the estimation as a fraction of the Nyquist
frequency.
mode: str, default "quaternion"
If "quaternion", compute the angular velocity from the quaternion
derivative. If "rotation_vector", compute the angular velocity from
the gradient of the axis-angle representation of the rotations.
allow_static: bool, default False
If True, return a zero velocity vector and None for timestamps if
the transform between this frame and the reference frame is static.
Otherwise, a `ValueError` will be raised.
return_timestamps: bool, default False
If True, also return the timestamps of the lookup.
Returns
-------
angular: numpy.ndarray, shape (N, 3)
Angular velocity of moving frame wrt reference frame, represented
in representation frame.
timestamps: each numpy.ndarray
Timestamps of the angular velocity.
"""
try:
reference = _resolve_rf(reference or self.parent)
represent_in = _resolve_rf(represent_in or self.parent)
except TypeError:
raise ValueError(f"Frame {self.name} has no parent frame")
_, rotation, timestamps = self.lookup_transform(reference)
if timestamps is None:
if allow_static:
return np.zeros(3), None
else:
raise ValueError(
"Velocity cannot be estimated for static transforms")
angular = _estimate_angular_velocity(
rotation,
timestamps,
cutoff=cutoff,
mode=mode,
outlier_thresh=outlier_thresh,
)
# angular velocity is represented in moving frame after estimation
angular, angular_ts = self.transform_vectors(
angular,
represent_in,
timestamps=timestamps,
return_timestamps=True,
)
if return_timestamps:
return angular, angular_ts
else:
return angular
def lookup_linear_velocity(
self,
reference=None,
represent_in=None,
outlier_thresh=None,
cutoff=None,
allow_static=False,
return_timestamps=False,
):
""" Estimate linear velocity of this frame wrt a reference.
Parameters
----------
reference: str or ReferenceFrame, optional
The reference frame wrt which the twist is estimated. Defaults to
the parent frame.
represent_in: str or ReferenceFrame, optional
The reference frame in which the twist is represented. Defaults
to the parent frame.
outlier_thresh: float, optional
Suppress outliers by throwing out samples where the
norm of the second-order differences of the position is above
`outlier_thresh` and interpolating the missing values.
cutoff: float, optional
Frequency of a low-pass filter applied to linear and angular
velocity after the estimation as a fraction of the Nyquist
frequency.
allow_static: bool, default False
If True, return a zero velocity vector and None for timestamps if
the transform between this frame and the reference frame is static.
Otherwise, a `ValueError` will be raised.
return_timestamps: bool, default False
If True, also return the timestamps of the lookup.
Returns
-------
linear: numpy.ndarray, shape (N, 3)
Linear velocity of moving frame wrt reference frame, represented
in representation frame.
timestamps: each numpy.ndarray
Timestamps of the linear velocity.
"""
try:
reference = _resolve_rf(reference or self.parent)
represent_in = _resolve_rf(represent_in or self.parent)
except TypeError:
raise ValueError(f"Frame {self.name} has no parent frame")
translation, _, timestamps = self.lookup_transform(reference)
if timestamps is None:
if allow_static:
return np.zeros(3), None
else:
raise ValueError(
"Velocity cannot be estimated for static transforms")
linear = _estimate_linear_velocity(
translation,
timestamps,
outlier_thresh=outlier_thresh,
cutoff=cutoff,
)
# linear velocity is represented in reference frame after estimation
linear, linear_ts = reference.transform_vectors(linear,
represent_in,
timestamps=timestamps,
return_timestamps=True)
if return_timestamps:
return linear, linear_ts
else:
return linear
