The newest Foxglove SDK helps robotics engineers to generate MCAP files for easy data logging and visualization. Now you can integrate into any code the generation and data capture for your robot.
In this tutorial, we are going to see how to capture images, both depth and RGB, as well as poses and transforms. The code is in Python, but the Foxglove SDK also has APIs for Rust and C++. The dataset used is from TheAirLab, in particular the Japanese Alley.
First of all, install the Foxglove SDK with pip
pip3 install foxglove-sdk
Now create a new folder to work on this dataset. It is recommended to have the following structure.
In the code
folder, create a new file called generate_mcap.py
.
Start by importing the necessary modules.
import os
import numpy as np
import cv2
import foxglove
from foxglove.channels import (
CompressedImageChannel,
PoseInFrameChannel,
PosesInFrameChannel,
FrameTransformChannel
)
from foxglove.schemas import (
Timestamp,
CompressedImage,
PosesInFrame,
PoseInFrame,
Pose,
Vector3,
Quaternion,
FrameTransform
)
Let’s also prepare the folders we will be using during the process
ROOT_FOLDER = os.path.abspath(os.path.join(
os.path.dirname(__file__), '..'))
LEFT_IMGS_FOLDER = os.path.join(ROOT_FOLDER, "P007", "image_left")
RIGHT_IMGS_FOLDER = os.path.join(ROOT_FOLDER, "P007", "image_right")
PNG_FOLDER = os.path.join(ROOT_FOLDER, "png")
if not os.path.exists(PNG_FOLDER):
os.makedirs(PNG_FOLDER)
OUTPUT_FOLDER = os.path.join(ROOT_FOLDER, "mcap_output")
if not os.path.exists(OUTPUT_FOLDER):
os.makedirs(OUTPUT_FOLDER)
Read each image as a file and store the data in the data
element of the CompressedImage
message. Then, log the message to the created channel. This function can be called for any folder containing png
images.
def generate_image_topic_from_folder(folder, topic_name) -> None:
"""
Generates a CompressedImage topic from a folder of images.
Args:
folder (str): Path to the folder containing images.
topic_name (str): Name of the topic to create.
"""
# Define the channel for compressed images
channel = CompressedImageChannel(topic=topic_name)
# Get names of images and sort
images = os.listdir(folder)
images.sort()
# Timespace initialization
ts = 0
total_imgs = len(images)
# For each image, read it, generate the message and log it
for i, image in enumerate(images):
image_path = os.path.join(folder, image)
with open(image_path, "rb") as file:
img = file.read()
data = img
# Get timest in seconds and convert to nanoseconds
ns = int(ts*1e9)
# Create a CompressedImage message
compressed_image = CompressedImage(
timestamp=Timestamp(0).from_epoch_secs(ts),
frame_id=topic_name,
format="png",
data=data
)
channel.log(
compressed_image,
log_time=ns,
)
# Increment the timestamp by 1/30 seconds (assuming 30 FPS)
ts += 1/30
# Print progress
print(f"{i}/{total_imgs} | {round(ts, 2)} | {ns}")
For the depth images, we will require an additional step in order to use the previously defined function. Depth images are stored as npy files, which are a type used by numpy . The following function reads each depth image and saves it as a png in a new folder.
def generate_png_from_npy(name) -> str:
"""
Generates PNG images from .npy files in the specified folder and saves them in a new folder.
Args:
name (str): Name (left or right) of the corresponding depth data folder.
Returns:
str: Path to the folder containing the generated PNG images.
"""
# Create the new folder for PNG images if it doesn't exist
os.makedirs(os.path.join(PNG_FOLDER, name), exist_ok=True)
folder = os.path.join(ROOT_FOLDER, "P007", f"depth_{name}")
# For each image, open with OpenCV and save as `png`
for img in os.listdir(folder):
img_path = os.path.join(folder, img)
data = np.load(img_path, allow_pickle=True)
cv2.imwrite(os.path.join(
PNG_FOLDER, name, img.replace(".npy", ".png")), data)
return os.path.join(PNG_FOLDER, name)
The poses at each moment are stored in txt
files that can be read with numpy
. For each pose, create the elements one by one. This will help with readability and generating different messages. These messages are Pose, PoseInFrame, PosesInFrame and FrameTransform.
def generate_pose_topic(name) -> None:
"""
Generates PoseInFrame, PosesInFrame, and FrameTransform topics from pose data in a text file.
Args:
name (str): Name of the pose data file (left or right).
"""
# Load the pose data from the text file
data = np.loadtxt(os.path.join("P007", f"pose_{name}.txt"))
# Define the channels for pose, poses, and frame transform
pose_channel = PoseInFrameChannel(f"/{name}_pose")
poses_channel = PosesInFrameChannel(f"/{name}_poses")
transform_channel = FrameTransformChannel(f"/{name}_transform")
# Initialize a list to store poses
poses = []
# Initialize the timestamp
ts = 0
# Iterate through the pose data and create messages
for i, data_pose in enumerate(data):
ns = int(ts*1e9)
# Create a variable for each element of the pose.
# This is useful for readability and generating different types of messages.
position = Vector3(
x=data_pose[0],
y=data_pose[1],
z=data_pose[2]
)
orientation = Quaternion(
x=data_pose[3],
y=data_pose[4],
z=data_pose[5],
w=data_pose[6]
)
pose_no_frame = Pose(
position=position,
orientation=orientation)
pose = PoseInFrame(
timestamp=Timestamp(0).from_epoch_secs(ts),
frame_id="odom",
pose=pose_no_frame)
# Log the pose
pose_channel.log(pose, log_time=ns)
# Append the new pose to the list of poses
poses.append(pose_no_frame)
path = PosesInFrame(
timestamp=Timestamp(0).from_epoch_secs(ts),
frame_id="odom",
poses=poses)
# Log the poses
poses_channel.log(path, log_time=ns)
# Create a FrameTransform message
frame_tf = FrameTransform(
timestamp=Timestamp(0).from_epoch_secs(ts),
parent_frame_id="odom",
child_frame_id=name+"_camera",
translation=position,
rotation=orientation)
# Log the frame transform
transform_channel.log(frame_tf, log_time=ns)
print(f"{i}/{len(data)} | {round(ts, 2)} | {ns}")
# Timestamp increment in accordance to the images (assuming 30 FPS)
ts += 1/30
Finish the code with the main execution. First, define the filename and path of the output mcap file. Then create the writer using the Foxglove SDK and finally call all the functions with the appropriate values. After this execution, an mcap file will appear in the output folder.
if __name__ == "__main__":
mcap_filename = "japanese_alley.mcap"
mcap_path = os.path.join(OUTPUT_FOLDER, mcap_filename)
writer = foxglove.open_mcap(mcap_path, allow_overwrite=True)
generate_image_topic_from_folder(LEFT_IMGS_FOLDER, "/image_left")
generate_image_topic_from_folder(RIGHT_IMGS_FOLDER, "/image_right")
generate_pose_topic("left")
generate_pose_topic("right")
left_png_path = generate_png_from_npy("left")
right_png_path = generate_png_from_npy("right")
generate_image_topic_from_folder(left_png_path, "/depth_left")
generate_image_topic_from_folder(right_png_path, "/depth_right")
writer.close()
print(f"MCAP file generated at: {mcap_path}")
Use Foxglove with the following layout to visualize the mcap file.
Using the new Foxglove SDK increases the capabilities of recording robotic data, without the need for ROS 2. Furthermore, it is compatible with the three most used programming languages used in robotics: C++, Python and Rust.
Join our community and keep in touch to learn more about this new tool that will help you log better data from your robots, learning about their behavior both in real time and after.