0
$\begingroup$

ROS2 Humble, Ubuntu 22.04

I have a ros2 bag which contains laserscan (~30Hz) and imu(~200Hz) messages. I am trying to do time synchronization of the two. I have seen several examples of such synchronization for live subscription for which a ROS 2 node is created. Since I am reading data from a rosbag using C++, I am wondering if I have to create a ROS node for that. My (attempted) code without a ROS node is as follows.

#include <iostream>
#include <memory>
#include <string>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <rosbag2_cpp/reader.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/cache.h>

using namespace std::placeholders;

void callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg, const sensor_msgs::msg::LaserScan::SharedPtr scan_msg) {
    std::cout << "Synchronized IMU and LaserScan messages received." << std::endl;
    // Process the synchronized messages here
}

int main(int argc, char** argv) {
    if (argc < 2) {
        std::cerr << "Please specify a bag file." << std::endl;
        return 1;
    }

    std::string bag_file = argv[1];

    // rclcpp::init(argc, argv); // Needed for serialization/deserialization

    using MySyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, sensor_msgs::msg::LaserScan>;
    message_filters::Subscriber<sensor_msgs::msg::Imu> imu_sub(nullptr, "/hai_xsens/filter/orientation_ros");  # NOT SURE IF nullptr works or not
    message_filters::Subscriber<sensor_msgs::msg::LaserScan> scan_sub(nullptr, "/hai_900_18/scan");
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), imu_sub, scan_sub);
    sync.registerCallback(&callback);

    rosbag2_cpp::Reader reader;
    reader.open(bag_file);

    while (reader.has_next()) {
        auto bag_message = reader.read_next();
        auto topic_name = bag_message->topic_name;

        if (topic_name == "imu") {
            auto imu_msg = std::make_shared<sensor_msgs::msg::Imu>();
            rclcpp::SerializedMessage serialized_msg(*bag_message->serialized_data);
            rclcpp::Serialization<sensor_msgs::msg::Imu> serialization;
            serialization.deserialize_message(&serialized_msg, imu_msg.get());
            // imu_sub.add(imu_msg);  # DOES NOT WORK
        } else if (topic_name == "scan") {
            auto scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
            rclcpp::SerializedMessage serialized_msg(*bag_message->serialized_data);
            rclcpp::Serialization<sensor_msgs::msg::LaserScan> serialization;
            serialization.deserialize_message(&serialized_msg, scan_msg.get());
            // scan_sub.add(scan_msg); # DOES NOT WORK
        }
    }

    return 0;
}

EDIT 1: The ros2 bag that I am using is provided here. The compiler error is as follows

In function ‘int main(int, char**)’:
/home/ash/ros2_ws/src/telesto_pkgs/fuse_laserscan/src/bag_synchroniser.cpp:51:25: error: cannot convert ‘std::shared_ptr<sensor_msgs::msg::Imu_<std::allocator<void> > >’ to ‘const EventType&’ {aka ‘const message_filters::MessageEvent<const sensor_msgs::msg::Imu_<std::allocator<void> > >&’}
   51 |             imu_sub.add(imu_msg);
      |                         ^~~~~~~
      |                         |
      |                         std::shared_ptr<sensor_msgs::msg::Imu_<std::allocator<void> > >
In file included from /home/ash/ros2_ws/src/telesto_pkgs/fuse_laserscan/src/bag_synchroniser.cpp:9:
/opt/ros/humble/include/message_filters/message_filters/subscriber.h:340:29: note:   initializing argument 1 of ‘void message_filters::Subscriber<M, NodeType>::add(const EventType&) [with M = sensor_msgs::msg::Imu_<std::allocator<void> >; NodeType = rclcpp::Node; message_filters::Subscriber<M, NodeType>::EventType = message_filters::MessageEvent<const sensor_msgs::msg::Imu_<std::allocator<void> > >]’
  340 |   void add(const EventType& e)
      |            ~~~~~~~~~~~~~~~~~^
gmake[2]: *** [CMakeFiles/bag_synchroniser.dir/build.make:76: CMakeFiles/bag_synchroniser.dir/src/bag_synchroniser.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:279: CMakeFiles/bag_synchroniser.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< fuse_laserscan [5.09s, exited with code 2]

EDIT 2: My CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(bag_synchroniser)

set (CMAKE_CXX_STANDARD 17)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosbag2_cpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rosbag2_transport REQUIRED)
find_package(message_filters REQUIRED)

include_directories(
  include
)

add_executable(bag_synchroniser
  src/bag_synchroniser.cpp
)

ament_target_dependencies(bag_synchroniser
  "rclcpp"
  "message_filters"
  "rosbag2_cpp"
  "sensor_msgs"
  "rosbag2_transport"
)

install(TARGETS
  bag_synchroniser
  DESTINATION lib/${PROJECT_NAME}
)
ament_package()

EDIT 3: I modified the code but it still did not work

#include <iostream>
#include <memory>
#include <string>

#include <rosbag2_cpp/reader.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <rclcpp/rclcpp.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/message_event.h>

// using namespace message_filters;
int i = 1;
int imu_i = 1;
int scan_i = 1;

void callback(const sensor_msgs::msg::Imu::ConstSharedPtr& imu_msg, const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan_msg) {
    std::cout << "Synchronized IMU and LaserScan messages received." << std::endl;
    std::cout<<i<<" msg time diff: "<<imu_msg->header.stamp.sec - scan_msg->header.stamp.sec<<std::endl;
    ++i;
    // Process synchronized messages here
}

template<typename M>
message_filters::MessageEvent<M> createMessageEvent(const std::shared_ptr<M>& message) {
    return message_filters::MessageEvent<M>(message, rclcpp::Time(message->header.stamp));
}

template<typename T>
std::shared_ptr<T> deserializeMessage(const rosbag2_storage::SerializedBagMessage &bag_message) {
    auto message = std::make_shared<T>();
    rclcpp::SerializedMessage serialized_msg(*bag_message.serialized_data);
    rclcpp::Serialization<T> serialization;
    serialization.deserialize_message(&serialized_msg, message.get());
    return message;
}

int main(int argc, char** argv) {
    if (argc < 2) {
        std::cerr << "Please specify a bag file." << std::endl;
        return 1;
    }

    std::string bag_file = argv[1];

    // rclcpp::init(argc, argv); // Initialize ROS 2

    // Define the synchronization policy and time synchronizer
    using SyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, sensor_msgs::msg::LaserScan>;
    message_filters::Subscriber<sensor_msgs::msg::Imu> imu_sub(nullptr, "/hai_xsens/filter/orientation_ros");  // NOT SURE IF nullptr works or not
    message_filters::Subscriber<sensor_msgs::msg::LaserScan> scan_sub(nullptr, "/hai_900_18/scan");
    message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), imu_sub, scan_sub);
    sync.registerCallback(&callback);

    // Open the ROS 2 bag for reading
    rosbag2_cpp::Reader reader;
    reader.open(bag_file);

    // Read and process messages from the bag
    while (reader.has_next()) {
        auto bag_message = reader.read_next();
        auto topic_name = bag_message->topic_name;

        if (topic_name == "/hai_xsens/filter/orientation_ros") {
            auto imu_msg = deserializeMessage<sensor_msgs::msg::Imu>(*bag_message);
            auto imu_event = createMessageEvent(imu_msg);
            imu_sub.add(imu_event);
            // std::cout<< "num imu msgs: "<<imu_i << std::endl;
            // ++imu_i;
        } else if (topic_name == "/hai_900_18/scan") {
            auto scan_msg = deserializeMessage<sensor_msgs::msg::LaserScan>(*bag_message);
            auto scan_event = createMessageEvent(scan_msg);
            scan_sub.add(scan_event); // Add LaserScan message event to the synchronizer
            // std::cout<< "num scan msgs: "<<scan_i << std::endl;
            // ++scan_i;
        }
    }

    // rclcpp::shutdown(); // Shutdown ROS 2

    return 0;
}

This time struck with Segmentation fault

Thanks

$\endgroup$
2
  • $\begingroup$ Please edit your question to include what "# DOES NOT WORK" about your problem. What's the compiler error? Can you provide a minimum reproducible example of your problem. $\endgroup$
    – Tully
    Commented Jul 9 at 18:40
  • $\begingroup$ Thanks @Tully. I have added and EDIT in my question with a link to the rosbag I use and the compiler error $\endgroup$
    – ASH
    Commented Jul 9 at 20:08

1 Answer 1

0
$\begingroup$

If you look at the error message. You need to pass the message filters add an event which includes the message, not the underlying message itself. The message filters rely on the extra metadata. You should be able to construct it to pass into the message filters.

The API is documented here: https://docs.ros.org/en/ros2_packages/rolling/api/message_filters/generated/classmessage__filters_1_1MessageEvent.html

$\endgroup$
2
  • $\begingroup$ Thanks @Tully for your time but unfortunately, even with MessageEvent, it's not working. Segmentation fault. I have added EDIT 3 above. $\endgroup$
    – ASH
    Commented Jul 11 at 9:51
  • $\begingroup$ There's a lot of potential causes of segfaults. A compile error is very different than a segfault. This question has effectively been answered. Please ask a new question with appropriate debugging for your segfault such as a stacktrace and attaching a debugger and showing the output. A quick browse of your code looks like you're creating the message event on the stack and then passing it to the message filter to work on it, but then it immediately goes out of scope which looks like it might be an issue. $\endgroup$
    – Tully
    Commented Jul 12 at 21:07

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.