I'm trying to plot the trajectory of a Crazyflie2 drone which is record on the /tf topic of a rosbag. I reused the code copied the code from this ROS2 tutorial (https://docs.ros.org/en/rolling/Tutorials/Advanced/Reading-From-A-Bag-File-CPP.html) and changed very little. But when I try to run my code on the appropriate rosbag, I get this :
[INFO] [1698913142.799724306] [rosbag2_storage]: Opened database 'ros2_ws/bagfiles/tfbag/tfbag_0.db3' for READ_ONLY.
>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:
'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:111'
with this new error message:
'Fast CDR exception deserializing message of type geometry_msgs::msg::dds_::TransformStamped_., at ./src/type_support_common.cpp:118'
rcutils_reset_error() should be called after error handling to avoid this.
<<<
terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
what(): Failed to deserialize ROS message.: Fast CDR exception deserializing message of type geometry_msgs::msg::dds_::TransformStamped_., at ./src/type_support_common.cpp:118
[ros2run]: Aborted
I'm pretty new to ROS2 and rclcpp, so I'm lost. Can someone help me out with this ?
My code :
#include <chrono>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <typeinfo>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/serialization.hpp"
#include "rosbag2_cpp/reader.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp" //
using namespace std::chrono_literals;
class PlaybackNode : public rclcpp::Node
{
public:
PlaybackNode(const std::string & bag_filename)
: Node("playback_node")
{
publisher_ = this->create_publisher<geometry_msgs::msg::TransformStamped>("/tf", 10);
timer_ = this->create_wall_timer(
100ms, std::bind(&PlaybackNode::timer_callback, this));
reader_.open(bag_filename);
}
private:
void timer_callback()
{
while (reader_.has_next()) {
rosbag2_storage::SerializedBagMessageSharedPtr msg = reader_.read_next();
if(msg->topic_name != "/tf"){
continue;
};
rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
geometry_msgs::msg::TransformStamped::SharedPtr ros_msg = std::make_shared<geometry_msgs::msg::TransformStamped>();
serialization_.deserialize_message(&serialized_msg, ros_msg.get());
publisher_->publish(*ros_msg);
int timestamp = ros_msg->header.stamp.sec + 0.000000001*(ros_msg->header.stamp.nanosec);
std::cout << "/tf [" << timestamp << "] : " << "(" << ros_msg->transform.translation.x
<< " ; " << ros_msg->transform.translation.y << " ; " << ros_msg->transform.translation.z << ")" << std::endl;
break;
}
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<geometry_msgs::msg::TransformStamped>::SharedPtr publisher_;
rclcpp::Serialization<geometry_msgs::msg::TransformStamped> serialization_;
rosbag2_cpp::Reader reader_;
};
int main(int argc, char ** argv)
{
if (argc != 2) {
std::cerr << "Usage: " << argv[0] << " <bag>" << std::endl;
return 1;
}
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<PlaybackNode>(argv[1]));
rclcpp::shutdown();
return 0;
}