0
$\begingroup$

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;
}
$\endgroup$

0

Your Answer

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

Browse other questions tagged or ask your own question.