def read_bag_trajectory(bag_handle, topic: str) -> PoseTrajectory3D:
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: trajectory topic of supported message type,
or a TF trajectory ID (e.g.: '/tf:map.base_link' )
:return: trajectory.PoseTrajectory3D
"""
from evo.tools import tf_cache
# Use TfCache instead if it's a TF transform ID.
if tf_cache.instance().check_id(topic):
return tf_cache.instance().get_trajectory(bag_handle, identifier=topic)
if not bag_handle.get_message_count(topic) > 0:
raise FileInterfaceException("no messages for topic '" + topic +
"' in bag")
msg_type = bag_handle.get_type_and_topic_info().topics[topic].msg_type
if msg_type not in SUPPORTED_ROS_MSGS:
raise FileInterfaceException(
"unsupported message type: {}".format(msg_type))
# Choose appropriate message conversion.
if msg_type == "geometry_msgs/TransformStamped":
get_xyz_quat = _get_xyz_quat_from_transform_stamped
else:
get_xyz_quat = _get_xyz_quat_from_pose_or_odometry_msg
stamps, xyz, quat = [], [], []
for topic, msg, _ in bag_handle.read_messages(topic):
# Use the header timestamps (converted to seconds).
t = msg.header.stamp
stamps.append(t.secs + (t.nsecs * 1e-9))
xyz_t, quat_t = get_xyz_quat(msg)
xyz.append(xyz_t)
quat.append(quat_t)
logger.debug("Loaded {} {} messages of topic: {}".format(
len(stamps), msg_type, topic))
generator = bag_handle.read_messages(topic)
_, first_msg, _ = next(generator)
frame_id = first_msg.header.frame_id
return PoseTrajectory3D(np.array(xyz),
np.array(quat),
np.array(stamps),
meta={"frame_id": frame_id})
def read_bag_trajectory(reader: typing.Union[Rosbag1Reader, Rosbag2Reader],
topic: str) -> PoseTrajectory3D:
"""
:param reader: opened bag reader (rosbags.rosbag2 or rosbags.rosbag1)
:param topic: trajectory topic of supported message type,
or a TF trajectory ID (e.g.: '/tf:map.base_link' )
:return: trajectory.PoseTrajectory3D
"""
if not isinstance(reader, (Rosbag1Reader, Rosbag2Reader)):
raise FileInterfaceException(
"reader must be a rosbags.rosbags1.reader.Reader "
"or rosbags.rosbags2.reader.Reader - "
"rosbag.Bag() is not supported by evo anymore")
# TODO: Support TF also with ROS2 bags.
if tf_id.check_id(topic):
if isinstance(reader, Rosbag1Reader):
# Use TfCache instead if it's a TF transform ID.
from evo.tools import tf_cache
return tf_cache.instance().get_trajectory(reader, identifier=topic)
else:
raise FileInterfaceException(
"TF support for ROS2 bags is not implemented")
if topic not in reader.topics:
raise FileInterfaceException("no messages for topic '" + topic +
"' in bag")
msg_type = reader.topics[topic].msgtype
if msg_type not in SUPPORTED_ROS_MSGS:
raise FileInterfaceException(
"unsupported message type: {}".format(msg_type))
# Choose appropriate message conversion.
if msg_type == "geometry_msgs/msg/TransformStamped":
get_xyz_quat = _get_xyz_quat_from_transform_stamped
else:
get_xyz_quat = _get_xyz_quat_from_pose_or_odometry_msg
stamps, xyz, quat = [], [], []
connections = [c for c in reader.connections if c.topic == topic]
for connection, _, rawdata in reader.messages(
connections=connections): # type: ignore
if isinstance(reader, Rosbag1Reader):
msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype),
connection.msgtype)
else:
msg = deserialize_cdr(rawdata, connection.msgtype)
# Use the header timestamps (converted to seconds).
# Note: msg/stamp is a rosbags type here, not native ROS.
t = msg.header.stamp
stamps.append(t.sec + (t.nanosec * 1e-9))
xyz_t, quat_t = get_xyz_quat(msg)
xyz.append(xyz_t)
quat.append(quat_t)
logger.debug("Loaded {} {} messages of topic: {}".format(
len(stamps), msg_type, topic))
# yapf: disable
(connection, _, rawdata) = list(reader.messages(connections=connections))[0] # type: ignore
# yapf: enable
if isinstance(reader, Rosbag1Reader):
first_msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype),
connection.msgtype)
else:
first_msg = deserialize_cdr(rawdata, connection.msgtype)
frame_id = first_msg.header.frame_id
return PoseTrajectory3D(np.array(xyz), np.array(quat), np.array(stamps),
meta={"frame_id": frame_id})