facontidavide / rosx_introspection

Deserialize any ROS message, without compilation time information.
Mozilla Public License 2.0
43 stars 5 forks source link

FastCDR throw exception when deserializeIntoJson `/rosout` topic #10

Open uupks opened 3 days ago

uupks commented 3 days ago

FastCDR throw exception when deserializeIntoJson /rosout@rcl_interfaces/msg/Log, while deserialize to flat container works well..

terminate called after throwing an instance of 'eprosima::fastcdr::exception::NotEnoughMemoryException'
 what(): Not enough memory in the buffer stream

Here are my test code :

#include "rosx_introspection/ros_parser.hpp"
#include "rosx_introspection/ros_utils/ros2_helpers.hpp"

#include "rclcpp/rclcpp.hpp"

int main(int argc, char const *argv[])
{
 rclcpp::InitOptions init_options;
 init_options.auto_initialize_logging(false);

 rclcpp::init(argc, argv, init_options);

 rclcpp::NodeOptions node_options;
 node_options.start_parameter_event_publisher(false);
 node_options.start_parameter_services(false);

 auto node = std::make_shared<rclcpp::Node>("ros2tui", node_options);

 std::string topic_name = "/rosout";
 std::string topic_type = "rcl_interfaces/msg/Log";

 RosMsgParser::Parser parser(topic_name, RosMsgParser::ROSType(topic_type), RosMsgParser::GetMessageDefinition(topic_type));

 auto sub = node->create_generic_subscription(
 topic_name, 
 topic_type, 
 1,
 [&] (std::shared_ptr<const rclcpp::SerializedMessage> message) {

 auto data = message->get_rcl_serialized_message().buffer;
 auto length = message->get_rcl_serialized_message().buffer_length;
 std::cout<<"Received message of length "<<length<<"\n";
 std::vector<uint8_t> buffer(data, data + length);
 RosMsgParser::ROS2_Deserializer deserializer;
 std::string json_text;
 parser.deserializeIntoJson(buffer, &json_text, &deserializer);
 std::cout << "\n JSON encoding :\n" << json_text << std::endl;
 });

 rclcpp::spin(node);
 rclcpp::shutdown();
 return 0;
}