search

lucasw / ros_one2z

Similar node graphs in vanilla ros1 and ros2 and also zenoh, focus on localhost using python but also C++ and maybe rust
BSD 3-Clause "New" or "Revised" License
5 stars 0 forks source link

ros2 mcap record and playback #8

Open lucasw opened 10 months ago

lucasw commented 10 months ago

can't find_package(mcap REQUIRED), the C++ library is header only son only need to get the include path in:

MAKEFLAGS="-j 4" colcon build --packages-select ros2_example_cpp --cmake-args -DCMAKE_CXX_STANDARD_INCLUDE_DIRECTORIES=$DEST/include
lucasw commented 10 months ago

How to set qos profile for each topic? There is metadata in the mcap:

      for (const auto& it : reader.channels()) {
        const auto channel = it.second;
        const auto schema = reader.schema(channel->schemaId);
        std::cout << channel->topic << " " << channel->messageEncoding << " "
            << schema->id << " " << schema->name << " " << schema->encoding << "\n";
        // TODO(lucasw) 'reliability' is in here but not sure about the mapping
        for (const auto& m_it : channel->metadata) {
          std::cout << "  " << m_it.first << ": "<< m_it.second << "\n";
        }
...
/rosout cdr 1 rcl_interfaces/msg/Log ros2msg
  topic_type_hash: RIHS01_e28ce254ca8abc06abf92773b74602cdbf116ed34fbaf294fb9f81da9f318eac
  offered_qos_profiles: - history: 3
  depth: 0
  reliability: 1
  durability: 1
  deadline:
    sec: 9223372036
    nsec: 854775807
  lifespan:
    sec: 10
    nsec: 0
  liveliness: 1
  liveliness_lease_duration:
    sec: 9223372036
    nsec: 854775807
  avoid_ros_namespace_conventions: false
- history: 3
  depth: 0
  reliability: 1
  durability: 1
  deadline:
    sec: 9223372036
    nsec: 854775807
  lifespan:
    sec: 10
    nsec: 0
  liveliness: 1
  liveliness_lease_duration:
    sec: 9223372036
    nsec: 854775807
  avoid_ros_namespace_conventions: false
- history: 3
  depth: 0
  reliability: 1
  durability: 1
  deadline:
    sec: 9223372036
    nsec: 854775807
  lifespan:
    sec: 10
    nsec: 0
  liveliness: 1
  liveliness_lease_duration:
    sec: 9223372036
    nsec: 854775807
  avoid_ros_namespace_conventions: false
- history: 3
  depth: 0
  reliability: 1
  durability: 1
  deadline:
    sec: 9223372036
    nsec: 854775807
  lifespan:
    sec: 10
    nsec: 0
  liveliness: 1
  liveliness_lease_duration:
    sec: 9223372036
    nsec: 854775807
  avoid_ros_namespace_conventions: false
- history: 3
  depth: 0
  reliability: 1
  durability: 1
  deadline:
    sec: 9223372036
    nsec: 854775807
  lifespan:
    sec: 10
    nsec: 0
  liveliness: 1
  liveliness_lease_duration:
    sec: 9223372036
    nsec: 854775807
  avoid_ros_namespace_conventions: false
lucasw commented 10 months ago

(why didn't this question https://answers.ros.org/question/376520/what-is-best-practice-for-setting-a-qos-profile-in-a-rclcpp-publisher-or-subscriber/ make it to robotics stackexchange - did it get retitled or deleted?

lucasw commented 10 months ago

when using my play_mcap

Type: geometry_msgs/msg/PolygonStamped

Publisher count: 1

Node name: play_mcap
Node namespace: /
Topic type: geometry_msgs/msg/PolygonStamped
Topic type hash: RIHS01_b7cf07932f1523d4b4088075945c1a0141f7cd21da87cc940fc61652e9138b46
Endpoint type: PUBLISHER
GID: 01.0f.15.f2.3f.a3.ca.c6.00.00.00.00.00.00.14.03
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0

and it errors with:

ros2 topic echo /contour
A message was lost!!!
    total count change:3
    total count: 3---
A message was lost!!!
    total count change:2
    total count: 5---
ros2 topic echo /contour
A message was lost!!!
    total count change:3
    total count: 3---
A message was lost!!!
    total count change:2
    total count: 5---

when using ros2 bag play

Type: geometry_msgs/msg/PolygonStamped

Publisher count: 1

Node name: rosbag2_player
Node namespace: /
Topic type: geometry_msgs/msg/PolygonStamped
Topic type hash: RIHS01_b7cf07932f1523d4b4088075945c1a0141f7cd21da87cc940fc61652e9138b46
Endpoint type: PUBLISHER
GID: 01.0f.15.f2.57.a2.4c.91.00.00.00.00.00.00.14.03
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0
ros2 topic echo /contour --raw
...
b'\x00\x01\x00\x00\xf9SFe\xca\xd9\x1e\x19\x04\x00\x00\x00map\x00d\x00\x00\x007\x89A?=\nW>\x00\x00\x00\x00\x9c\xc4@?\xac\x1cZ>\x00\x00\x00\x00\xf6(<?\xac\x1cZ>\x00\x00\x00\x00Zd;?\x1b/]>\x00\x00\x00\x00\x87\x169?\x1b/]>\x00\x00\x00\x00\xecQ8?\x89A`>\x00\x00\x00\x00P\x8d7?\x89A`>\x00\x00\x00\x00\xb4\xc86?\xf8Sc>\x00\x00\x00\x00\x19\x046?\xf8Sc>\x00\x00\x00\x00}?5?fff>\x00\x00\x00\x00\xe1z4?fff>\x00\x00\x00\x00\x0e-2?\xb2\x9do>\x00\x00\x00\x00sh1?\xb2\x9do>\x00\x00\x00\x00\xa0\x1a/?\xfe\xd4x>\x00\x00\x00\x00\xa0\x1a/?m\xe7{>\x00\x00\x00\x00\xcd\xcc,?\\\x8f\x82>\x00\x00\x00\x00\xcd\xcc,?\x93\x18\x84>\x00\x00\x00\x001\x08,?\xcb\xa1\x85>\x00\x00\x00\x001\x08,?\x02+\x87>\x00\x00\x00\x00\x96C+?9\xb4\x88>\x00\x00\x00\x00\x96C+?q=\x8a>\x00\x00\x00\x00\xfa~*?\xa8\xc6\x8b>\x00\x00\x00\x00\xfa~*?Nb\x90>\x00\x00\x00\x00^\xba)?\x85\xeb\x91>\x00\x00\x00\x00^\xba)?\xd1"\x9b>\x00\x00\x00\x00\xc3\xf5(?\x08\xac\x9c>\x00\x00\x00\x00^\xba)??5\x9e>\x00\x00\x00\x00^\xba)?\x8bl\xa7>\x00\x00\x00\x00\xfa~*?\xc3\xf5\xa8>\x00\x00\x00\x00\xfa~*?h\x91\xad>\x00\x00\x00\x00\x96C+?\xa0\x1a\xaf>\x00\x00\x00\x00\x96C+?\xd7\xa3\xb0>\x00\x00\x00\x001\x08,?\x0e-\xb2>\x00\x00\x00\x001\x08,?F\xb6\xb3>\x00\x00\x00\x00\xcd\xcc,?}?\xb5>\x00\x00\x00\x00\xcd\xcc,?\xb4\xc8\xb6>\x00\x00\x00\x00\xa0\x1a/?Zd\xbb>\x00\x00\x00\x00\xa0\x1a/?\x91\xed\xbc>\x00\x00\x00\x00sh1?7\x89\xc1>\x00\x00\x00\x00\x0e-2?7\x89\xc1>\x00\x00\x00\x00\xe1z4?\xdd$\xc6>\x00\x00\x00\x00}?5?\xdd$\xc6>\x00\x00\x00\x00\x19\x046?\x14\xae\xc7>\x00\x00\x00\x00\xb4\xc86?\x14\xae\xc7>\x00\x00\x00\x00P\x8d7?L7\xc9>\x00\x00\x00\x00\xecQ8?L7\xc9>\x00\x00\x00\x00\x87\x169?\x83\xc0\xca>\x00\x00\x00\x00Zd;?\x83\xc0\xca>\x00\x00\x00\x00\xf6(<?\xbaI\xcc>\x00\x00\x00\x00\x9c\xc4@?\xbaI\xcc>\x00\x00\x00\x007\x89A?\xf2\xd2\xcd>\x00\x00\x00\x00\xd3MB?\xbaI\xcc>\x00\x00\x00\x00y\xe9F?\xbaI\xcc>\x00\x00\x00\x00\x14\xaeG?\x83\xc0\xca>\x00\x00\x00\x00\xe7\xfbI?\x83\xc0\xca>\x00\x00\x00\x00\x83\xc0J?L7\xc9>\x00\x00\x00\x00\x1f\x85K?L7\xc9>\x00\x00\x00\x00\xbaIL?\x14\xae\xc7>\x00\x00\x00\x00V\x0eM?\x14\xae\xc7>\x00\x00\x00\x00\xf2\xd2M?\xdd$\xc6>\x00\x00\x00\x00\x8d\x97N?\xdd$\xc6>\x00\x00\x00\x00`\xe5P?7\x89\xc1>\x00\x00\x00\x00\xfc\xa9Q?7\x89\xc1>\x00\x00\x00\x00\xcf\xf7S?\x91\xed\xbc>\x00\x00\x00\x00\xcf\xf7S?Zd\xbb>\x00\x00\x00\x00\xa2EV?\xb4\xc8\xb6>\x00\x00\x00\x00\xa2EV?}?\xb5>\x00\x00\x00\x00=\nW?F\xb6\xb3>\x00\x00\x00\x00=\nW?\x0e-\xb2>\x00\x00\x00\x00\xd9\xceW?\xd7\xa3\xb0>\x00\x00\x00\x00\xd9\xceW?\xa0\x1a\xaf>\x00\x00\x00\x00u\x93X?h\x91\xad>\x00\x00\x00\x00u\x93X?\xc3\xf5\xa8>\x00\x00\x00\x00\x10XY?\x8bl\xa7>\x00\x00\x00\x00\x10XY??5\x9e>\x00\x00\x00\x00\xac\x1cZ?\x08\xac\x9c>\x00\x00\x00\x00\x10XY?\xd1"\x9b>\x00\x00\x00\x00\x10XY?\x85\xeb\x91>\x00\x00\x00\x00u\x93X?Nb\x90>\x00\x00\x00\x00u\x93X?\xa8\xc6\x8b>\x00\x00\x00\x00\xd9\xceW?q=\x8a>\x00\x00\x00\x00\xd9\xceW?9\xb4\x88>\x00\x00\x00\x00=\nW?\x02+\x87>\x00\x00\x00\x00=\nW?\xcb\xa1\x85>\x00\x00\x00\x00\xa2EV?\x93\x18\x84>\x00\x00\x00\x00\xa2EV?\\\x8f\x82>\x00\x00\x00\x00\xcf\xf7S?m\xe7{>\x00\x00\x00\x00\xcf\xf7S?\xfe\xd4x>\x00\x00\x00\x00\xfc\xa9Q?\xb2\x9do>\x00\x00\x00\x00`\xe5P?\xb2\x9do>\x00\x00\x00\x00\x8d\x97N?fff>\x00\x00\x00\x00\xf2\xd2M?fff>\x00\x00\x00\x00V\x0eM?\xf8Sc>\x00\x00\x00\x00\xbaIL?\xf8Sc>\x00\x00\x00\x00\x1f\x85K?\x89A`>\x00\x00\x00\x00\x83\xc0J?\x89A`>\x00\x00\x00\x00\xe7\xfbI?\x1b/]>\x00\x00\x00\x00\x14\xaeG?\x1b/]>\x00\x00\x00\x00y\xe9F?\xac\x1cZ>\x00\x00\x00\x00\xd3MB?\xac\x1cZ>\x00\x00\x00\x00'
lucasw commented 10 months ago

Need to set the rcl buffer length:

rclcpp::SerializedMessage serialized_msg;
serialized_msg.reserve(msg_size);
auto& rcl_msg = serialized_msg.get_rcl_serialized_message();
// TODO(lucasw) is this copy avoidable?
std::memcpy(rcl_msg.buffer, msgView.message.data, msg_size);
rcl_msg.buffer_length = msg_size;

pub->publish(serialized_msg);

https://robotics.stackexchange.com/questions/107510/how-to-read-a-mcap-file-and-publish-message-using-cpp-and-ros2foxy