Skip to content

Add ROS1 bag tools and canonical layout: bag_to_laz, laz_to_bag #582

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
michalpelka wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Add ROS1 bag tools and canonical layout: bag_to_laz, laz_to_bag #582

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b9a9c8c
Add ROS1 bag tools and canonical layout: bag_to_laz, laz_to_bag
michalpelka Oct 26, 2026
5117e14
mcap_to_laz/bag_to_laz: accept several inputs
michalpelka Oct 26, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
66 changes: 66 additions & 0 deletions apps/console_tools/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,72 @@ if (MSVC)
target_compile_options(mcap_to_laz PRIVATE /bigobj)
endif()

# bag_to_laz / laz_to_bag: the ROS1 bag twins of mcap_to_laz / laz_to_mcap,
# built from the same sources with a define that swaps the reader (writer) for
# rosbags/Ros1Bag -- no ROS installation needed. Reading bz2 chunks needs
# libbz2 (liblz4 comes with the mcap target), so both are skipped without it.
find_package(BZip2)
if (BZip2_FOUND)
add_executable(
bag_to_laz mcap_to_laz.cpp
${REPOSITORY_DIRECTORY}/rosbags/Ros1Bag.h ${REPOSITORY_DIRECTORY}/rosbags/Ros1Bag.cpp
${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp
${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.h ${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.cpp
)
target_compile_definitions(bag_to_laz PRIVATE HDMAPPING_IMPORT_ROS1_BAG)
target_include_directories(
bag_to_laz
PRIVATE ${REPOSITORY_DIRECTORY}/rosbags
${LASZIP_INCLUDE_DIR}/LASzip/include)
target_link_libraries(
bag_to_laz
PRIVATE
mcap
BZip2::BZip2
spdlog::spdlog
${PLATFORM_LASZIP_LIB}
${PLATFORM_MISCELLANEOUS_LIBS})

add_executable(
laz_to_bag laz_to_mcap.cpp
${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.h
${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp
${REPOSITORY_DIRECTORY}/rosbags/Ros1Bag.h ${REPOSITORY_DIRECTORY}/rosbags/Ros1Bag.cpp
${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp
)
target_compile_definitions(laz_to_bag PRIVATE HDMAPPING_EXPORT_ROS1_BAG)
target_include_directories(
laz_to_bag
PRIVATE ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1
${REPOSITORY_DIRECTORY}/core/include
${REPOSITORY_DIRECTORY}/rosbags
${THIRDPARTY_DIRECTORY} # csv.hpp (used by load_imu)
${THIRDPARTY_DIRECTORY}/glm
${EIGEN3_INCLUDE_DIR}
${THIRDPARTY_DIRECTORY}/tomlplusplus/include
${THIRDPARTY_DIRECTORY}/json/include
${LASZIP_INCLUDE_DIR}/LASzip/include
${THIRDPARTY_DIRECTORY}/observation_equations/codes
${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
target_link_libraries(
laz_to_bag
PRIVATE
mcap
BZip2::BZip2
unordered_dense::unordered_dense
spdlog::spdlog
${PLATFORM_LASZIP_LIB}
${PLATFORM_MISCELLANEOUS_LIBS}
${CORE_LIBRARIES})

if (MSVC)
target_compile_options(bag_to_laz PRIVATE /bigobj)
target_compile_options(laz_to_bag PRIVATE /bigobj)
endif()
else()
message(STATUS "bag_to_laz and laz_to_bag skipped: libbz2 not found")
endif()

# ouster_pcap_to_laz: Ouster pcap + metadata JSON -> mandeye session directory.
# Same writer as mcap_to_laz; the pcap/packet decoding is rosbags/OusterPcapReader,
# so laszip + nlohmann json + spdlog is the whole dependency set.
Expand Down
139 changes: 101 additions & 38 deletions apps/console_tools/laz_to_mcap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
// Standalone mandeye session directory -> MCAP/ROS2-bag exporter.
// Standalone mandeye session directory -> MCAP/ROS2-bag exporter. Built a
// second time as laz_to_bag (HDMAPPING_EXPORT_ROS1_BAG), which writes a ROS1 bag
// through rosbags/Ros1Bag instead -- same layouts, topics and message split.
//
// Reuses load_point_cloud()/load_imu() (lidar_odometry_utils.cpp) -- the
// same LAZ/IMU-csv parsing lidar_odometry_step_1 itself uses -- rather than
Expand All @@ -7,6 +9,9 @@
// entirely) that would silently drop data for a lossless exporter.
#include "McapWriter.h"
#include "lidar_odometry_utils.h"
#if defined(HDMAPPING_EXPORT_ROS1_BAG)
#include "Ros1Bag.h"
#endif

#include <algorithm>
#include <cmath>
Expand All @@ -25,6 +30,17 @@ using ImuData = std::vector<std::tuple<std::pair<double, double>, Eigen::Vector3
namespace
{

#if defined(HDMAPPING_EXPORT_ROS1_BAG)
using BagWriter = rosbags::Ros1BagFileWriter;
constexpr const char* kOutputExtension = ".bag";
#else
using BagWriter = rosbags::McapFileWriter;
constexpr const char* kOutputExtension = ".mcap";
#endif

// Standard gravity: the session's IMU csv is in g, sensor_msgs/Imu wants m/s^2.
constexpr double kStandardGravity = 9.80665;

bool check_path_ext(const std::string& path, const char* ext)
{
return fs::path(path).extension() == ext;
Expand All @@ -43,25 +59,38 @@ namespace
return s;
}

std::vector<rosbags::McapPoint> to_mcap_points(const std::vector<Point3Di>& points)
// `rings` is parallel to `points` (load_point_cloud()'s ring output). The result
// is sorted by timestamp, ties by ring: the order in which a spinning lidar
// fires, and the order every PointCloud2 layout's relative time is taken in.
std::vector<rosbags::McapPoint> to_mcap_points(const std::vector<Point3Di>& points, const std::vector<uint16_t>& rings)
{
std::vector<rosbags::McapPoint> out;
out.reserve(points.size());
for (const auto& p : points)
for (size_t i = 0; i < points.size(); ++i)
{
const auto& p = points[i];
rosbags::McapPoint mp{};
mp.x = static_cast<float>(p.point.x());
mp.y = static_cast<float>(p.point.y());
mp.z = static_cast<float>(p.point.z());
mp.intensity = p.intensity;
mp.ring = i < rings.size() ? rings[i] : 0;
mp.laser_id = p.lidarid;
mp.timestamp = p.timestamp;
out.push_back(mp);
}
std::stable_sort(
out.begin(),
out.end(),
[](const rosbags::McapPoint& a, const rosbags::McapPoint& b)
{
return a.timestamp < b.timestamp || (a.timestamp == b.timestamp && a.ring < b.ring);
});
return out;
}

std::vector<rosbags::McapImuSample> to_mcap_imu(const ImuData& imu_data)
// `acc_scale` brings the csv's acceleration to the unit written (1 keeps g).
std::vector<rosbags::McapImuSample> to_mcap_imu(const ImuData& imu_data, double acc_scale)
{
std::vector<rosbags::McapImuSample> out;
out.reserve(imu_data.size());
Expand All @@ -72,25 +101,14 @@ namespace
s.gyro_x = gyr.x();
s.gyro_y = gyr.y();
s.gyro_z = gyr.z();
s.acc_x = acc.x();
s.acc_y = acc.y();
s.acc_z = acc.z();
s.acc_x = static_cast<float>(acc.x() * acc_scale);
s.acc_y = static_cast<float>(acc.y() * acc_scale);
s.acc_z = static_cast<float>(acc.z() * acc_scale);
out.push_back(s);
}
return out;
}

void sort_points_by_timestamp(std::vector<Point3Di>& points)
{
std::sort(
points.begin(),
points.end(),
[](const Point3Di& a, const Point3Di& b)
{
return a.timestamp < b.timestamp;
});
}

void sort_imu_by_timestamp(ImuData& imu)
{
std::sort(
Expand All @@ -111,15 +129,15 @@ namespace
class MessageSplitter
{
public:
MessageSplitter(rosbags::McapFileWriter& writer, double msg_hz)
MessageSplitter(BagWriter& writer, double msg_hz)
: writer_(writer)
, msg_hz_(msg_hz)
{
}

// `points` must be sorted by timestamp, and successive calls must be in
// timestamp order too (session chunks are processed oldest-first).
void add(const std::vector<Point3Di>& points)
void add(const std::vector<rosbags::McapPoint>& points)
{
if (msg_hz_ <= 0.0)
{
Expand Down Expand Up @@ -149,19 +167,18 @@ namespace
}

private:
void write(const std::vector<Point3Di>& points)
void write(const std::vector<rosbags::McapPoint>& points)
{
if (points.empty())
return;
const auto mcap_points = to_mcap_points(points);
const uint64_t stamp_ns = static_cast<uint64_t>(mcap_points.front().timestamp * 1e9);
writer_.writePointCloud(stamp_ns, mcap_points);
const uint64_t stamp_ns = static_cast<uint64_t>(points.front().timestamp * 1e9);
writer_.writePointCloud(stamp_ns, points);
++messages_;
}

rosbags::McapFileWriter& writer_;
BagWriter& writer_;
double msg_hz_;
std::vector<Point3Di> pending_;
std::vector<rosbags::McapPoint> pending_;
int64_t current_bin_ = 0;
size_t messages_ = 0;
};
Expand Down Expand Up @@ -315,7 +332,7 @@ namespace

void print_usage(const char* argv0)
{
spdlog::error("Usage: {} <session_dir> <output.mcap> [options]", argv0);
spdlog::error("Usage: {} <session_dir> <output{}> [options]", argv0, kOutputExtension);
spdlog::error(" session_dir a mandeye recording folder of lidarNNNN.laz + imuNNNN.csv chunk");
spdlog::error(" pairs (matched by filename's last 4 digits); every chunk becomes");
spdlog::error(" its own /lidar_points message, IMU samples are merged into one stream");
Expand All @@ -324,20 +341,43 @@ namespace
spdlog::error(" --imu-topic <name> IMU topic (default: /imu)");
spdlog::error(" --sn-topic <name> serial-number string topic (default: /lidar_sn)");
spdlog::error(" --frame-id <name> frame_id written into message headers (default: lidar)");
spdlog::error(" --lidar-type <type> PointCloud2 field layout: generic|velodyne|ouster|hesai (default: generic)");
spdlog::error(" --lidar-type <type> PointCloud2 field layout: generic|velodyne|ouster|hesai|canonical");
spdlog::error(" (default: generic); canonical = x y z intensity ring time(float32) t,");
spdlog::error(" the benchmark's canonical spinning-lidar bag");
spdlog::error(" --imu-acc-unit <g|ms2> unit IMU acceleration is written in (default: g, the session's own,");
spdlog::error(" copied as is; ms2 = m/s^2 as sensor_msgs/Imu defines it)");
spdlog::error(" --msg_hz <float> message rate: points are split into one PointCloud2 per");
spdlog::error(" 1/hz seconds (default: 10; 0 = one message per input file/chunk)");
spdlog::error(" --calibration <file> MLvxCalib calibration.json/.mjc (multi-LiVoX extrinsics + IMU");
spdlog::error(" selection); session_dir auto-detects this if omitted");
spdlog::error(" --sn <file> MLvxCalib .sn file (sensor id -> serial number); session_dir");
spdlog::error(" auto-detects this if omitted. Must be given together with --calibration");
spdlog::error("The lidar ring is read from the LAS extra-bytes attribute 'ring' when the session has it");
spdlog::error("(mcap_to_laz and bag_to_laz write it), otherwise every point's ring is 0.");
}

// The ROS1 bag writer finishes its index explicitly so a write error is not lost
// in a destructor; MCAP finishes on destruction.
bool finish(rosbags::McapFileWriter&)
{
return true;
}
#if defined(HDMAPPING_EXPORT_ROS1_BAG)
bool finish(rosbags::Ros1BagFileWriter& writer)
{
if (writer.close())
return true;
spdlog::error("Failed to finish the bag: {}", writer.error());
return false;
}
#endif

int run_session_directory(
const std::string& dir_path,
const std::string& mcap_path,
const rosbags::McapWriterOptions& options,
double msg_hz,
double acc_scale,
std::string calibration_path,
std::string sn_path)
{
Expand All @@ -360,15 +400,17 @@ namespace
}
const Calibration calib = load_calibration(calibration_path, sn_path);

rosbags::McapFileWriter writer(mcap_path, options);
BagWriter writer(mcap_path, options);
if (!writer.isOpen())
{
spdlog::error("Failed to open output mcap file {}", mcap_path);
spdlog::error("Failed to open output file {}", mcap_path);
return EXIT_FAILURE;
}

size_t total_points = 0;
size_t points_with_ring = 0;
ImuData all_imu;
std::vector<uint16_t> rings;
MessageSplitter splitter(writer, msg_hz);
for (size_t i = 0; i < chunks.size(); ++i)
{
Expand All @@ -377,10 +419,11 @@ namespace
/*ommit_points_with_timestamp_equals_zero=*/false,
/*filter_threshold_xy_inner=*/0.0,
/*filter_threshold_xy_outer=*/std::numeric_limits<double>::max(),
/*calibrations=*/calib.per_sensor);
sort_points_by_timestamp(points);
/*calibrations=*/calib.per_sensor,
/*rings=*/&rings);
total_points += points.size();
splitter.add(points);
points_with_ring += static_cast<size_t>(std::count_if(rings.begin(), rings.end(), [](uint16_t r) { return r != 0; }));
splitter.add(to_mcap_points(points, rings));
spdlog::info("[{}/{}] {}: {} points", i + 1, chunks.size(), chunks[i].laz, points.size());

if (!chunks[i].imu.empty())
Expand All @@ -395,14 +438,18 @@ namespace
total_points,
chunks.size(),
splitter.messages_written());
if (points_with_ring == 0 && total_points > 0)
spdlog::info("No point has a ring (the session has no 'ring' attribute, or every ring is 0)");

if (!all_imu.empty())
{
sort_imu_by_timestamp(all_imu);
writer.writeImu(to_mcap_imu(all_imu));
writer.writeImu(to_mcap_imu(all_imu, acc_scale));
spdlog::info("Loaded {} IMU samples across matched chunk(s)", all_imu.size());
}

if (!finish(writer))
return EXIT_FAILURE;
spdlog::info("Wrote {}", mcap_path);
return EXIT_SUCCESS;
}
Expand All @@ -423,6 +470,7 @@ int main(const int argc, const char** argv)
std::string sn_path;
rosbags::McapWriterOptions options;
double msg_hz = 10.0;
double acc_scale = 1.0;

for (int i = 3; i < argc; ++i)
{
Expand Down Expand Up @@ -469,9 +517,24 @@ int main(const int argc, const char** argv)
options.lidar_layout = rosbags::PointCloudLayout::Ouster;
else if (type == "hesai")
options.lidar_layout = rosbags::PointCloudLayout::Hesai;
else if (type == "canonical")
options.lidar_layout = rosbags::PointCloudLayout::Canonical;
else
{
spdlog::error("Unknown --lidar-type '{}' (expected generic|velodyne|ouster|hesai|canonical)", type);
return EXIT_FAILURE;
}
}
else if (arg == "--imu-acc-unit" && hasValue)
{
const std::string unit = argv[++i];
if (unit == "g")
acc_scale = 1.0;
else if (unit == "ms2")
acc_scale = kStandardGravity;
else
{
spdlog::error("Unknown --lidar-type '{}' (expected generic|velodyne|ouster|hesai)", type);
spdlog::error("Unknown --imu-acc-unit '{}' (expected g|ms2)", unit);
return EXIT_FAILURE;
}
}
Expand All @@ -483,9 +546,9 @@ int main(const int argc, const char** argv)
}
}

if (!check_path_ext(mcap_path, ".mcap"))
if (!check_path_ext(mcap_path, kOutputExtension))
{
spdlog::error("Invalid extension for output file {} - expected .mcap", mcap_path);
spdlog::error("Invalid extension for output file {} - expected {}", mcap_path, kOutputExtension);
return EXIT_FAILURE;
}
if (!fs::exists(input_path))
Expand All @@ -504,5 +567,5 @@ int main(const int argc, const char** argv)
return EXIT_FAILURE;
}

return run_session_directory(input_path, mcap_path, options, msg_hz, calibration_path, sn_path);
return run_session_directory(input_path, mcap_path, options, msg_hz, acc_scale, calibration_path, sn_path);
}
Loading
Loading