Skip to content

ROS Bags

rosbag

geometry_point_stamped_handler 

geometry_point_stamped_handler(
    msg: PointMessage,
) -> dict[str, Any]

Handler for geometry_msgs/msg/PointStamped messages

Parameters:

  • msg (Any) –

    Message to handle

Returns:

  • dict[str, Any] –

    dict[str, Any]: Dictionary containing the data

Source code in trajectopy\io\rosbag.py 
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
def geometry_point_stamped_handler(msg: PointMessage) -> dict[str, Any]:
    """Handler for geometry_msgs/msg/PointStamped messages

    Args:
        msg (Any): Message to handle

    Returns:
        dict[str, Any]: Dictionary containing the data
    """
    return {
        "tstamps": msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9,
        "x": msg.point.x,
        "y": msg.point.y,
        "z": msg.point.z,
    }

trajectories_from_rosbag 

trajectories_from_rosbag(filename: str) -> list[Trajectory]

Creates a trajectory from a ROS bag file

Parameters:

  • filename (str) –

    File to read

Returns:

  • Trajectory ( list[Trajectory] ) –

    Trajectory created from the ROS bag file

Source code in trajectopy\io\rosbag.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
def trajectories_from_rosbag(filename: str) -> list[Trajectory]:
    """Creates a trajectory from a ROS bag file

    Args:
        filename (str): File to read

    Returns:
        Trajectory: Trajectory created from the ROS bag file
    """
    data = read_ros_bag(filename)

    trajectories = []
    for traj_key, traj_data in data.items():
        pos = Positions(xyz=np.c_[traj_data["x"], traj_data["y"], traj_data["z"]])

        if any(key not in traj_data for key in ["qx", "qy", "qz", "qw"]):
            rot = None
            logger.info("No rotation data found for trajectory %s", traj_key)
        else:
            rot = Rotations.from_quat(quat=np.c_[traj_data["qx"], traj_data["qy"], traj_data["qz"], traj_data["qw"]])

        tstamps = np.array(traj_data.get("tstamps", np.arange(len(pos))), dtype=float)

        trajectories.append(Trajectory(name=traj_key, positions=pos, rotations=rot, timestamps=tstamps))
        logger.info("Found trajectory: %s", traj_key)

    logger.info("Created %d trajectories from ROS bag file", len(trajectories))
    return trajectories

read_ros_bag 

read_ros_bag(
    filename: str,
) -> dict[str, dict[str, list[Any]]]

Reads a ROS bag file and returns the header and the data

Parameters:

  • filename (str) –

    File to read

Returns:

  • dict[str, dict[str, list[Any]]] –

    Tuple[HeaderData, np.ndarray]: Header data and data

Source code in trajectopy\io\rosbag.py 
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
def read_ros_bag(filename: str) -> dict[str, dict[str, list[Any]]]:
    """Reads a ROS bag file and returns the header and the data

    Args:
        filename (str): File to read

    Returns:
        Tuple[HeaderData, np.ndarray]: Header data and data
    """
    ROS_MESSAGE_HANDLERS: dict[str, Callable[[Any], dict[str, Any]]] = {
        "geometry_msgs/msg/PoseStamped": geometry_pose_stamped_handler,
        "geometry_msgs/msg/PointStamped": geometry_point_stamped_handler,
    }
    data: dict[str, dict[str, list[Any]]] = {}
    with AnyReader([Path(filename)], default_typestore=typestore) as reader:
        for connection, _, rawdata in reader.messages(connections=reader.connections):
            msg = reader.deserialize(rawdata, connection.msgtype)

            msg_dict = ROS_MESSAGE_HANDLERS.get(connection.msgtype)
            if msg_dict is None:
                logger.warning("Message type %s not supported", connection.msgtype)
                continue

            for key, value in msg_dict(msg).items():
                add_to_dict(data, connection.topic, key, value)

    return data

add_to_dict 

add_to_dict(
    data: dict[str, dict[str, list[Any]]],
    key: str,
    sub_key: str,
    value: Any,
) -> None

Adds a value to a dictionary

Parameters:

  • data (dict) –

    Dictionary to add the value to

  • key (str) –

    Key to add the value to

  • value (Any) –

    Value to add

Source code in trajectopy\io\rosbag.py 
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
def add_to_dict(data: dict[str, dict[str, list[Any]]], key: str, sub_key: str, value: Any) -> None:
    """Adds a value to a dictionary

    Args:
        data (dict): Dictionary to add the value to
        key (str): Key to add the value to
        value (Any): Value to add
    """
    if key not in data:
        data[key] = {}

    if sub_key not in data[key]:
        data[key][sub_key] = []

    data[key][sub_key].append(value)