search

eProsima / Micro-XRCE-DDS-Client

Micro XRCE-DDS Client repository. Looking for commercial support? Contact info@eprosima.com
Apache License 2.0
130 stars 82 forks source link

Topic published from MCU client cannot be echoed by ROS2 #377

Closed Pansamic closed 8 months ago

Pansamic commented 8 months ago

Problem

ros2 topic echo /imu/data shows nothing, but ros2 topic list shows correct topic /imu/data.

Detail

uxrCustomTransport transport; uxrSession session; uxrStreamId reliable_out;

uxrObjectId participant_id; uint16_t participant_req; uxrObjectId datawriter_id; uint16_t datawriter_req;

ucdrBuffer mb;

sensor_msgs_msg_Imu msg_imu = {0};

uint8_t output_reliable_stream_buffer[STREAM_BUFFER_SIZE]; uint8_t input_reliable_stream_buffer[STREAM_BUFFER_SIZE]; /**/ / DECLARATION / /**/

/**/ / DEFINITION / /**/ uint8_t uxrce_client_init() { // Transport uxr_set_custom_transport_callbacks( &transport, true, my_custom_transport_open, my_custom_transport_close, my_custom_transport_write, my_custom_transport_read); if (!uxr_init_custom_transport(&transport, NULL)) { printf("Error at create transport.\n"); return 1; } // Session

uxr_init_session(&session, &transport.comm, 0xAAAABBBB);
uxr_set_topic_callback(&session, uxr_onTopicCallback, NULL);
if(!uxr_create_session(&session))
{
    printf("Error at create session.\n");
    return 1;
}

// Streams
reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, STREAM_BUFFER_SIZE, STREAM_HISTORY);
uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, STREAM_BUFFER_SIZE, STREAM_HISTORY);
    // Create entities
participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID);
participant_req = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, 3, "nav_mecanum_driver", UXR_REPLACE|UXR_REUSE);

uxr_sync_session(&session,1000);
return 0;

}

uint8_t create_publisher(const char topic_name, const char data_type) { static uint16_t publisher_index = 0;

uxrObjectId topic_id = uxr_object_id(publisher_index, UXR_TOPIC_ID);
// uint16_t topic_req = uxr_buffer_create_topic_xml(&session, reliable_out, topic_id, participant_id, topic_xml, UXR_REPLACE|UXR_REUSE);
uint16_t topic_req = uxr_buffer_create_topic_bin(&session, reliable_out, topic_id, participant_id, topic_name, data_type, UXR_REPLACE|UXR_REUSE);

uxrObjectId publisher_id = uxr_object_id(publisher_index, UXR_PUBLISHER_ID);
// uint16_t publisher_req = uxr_buffer_create_publisher_xml(&session, reliable_out, publisher_id, participant_id, publisher_xml, UXR_REPLACE|UXR_REUSE);
uint16_t publisher_req = uxr_buffer_create_publisher_bin(&session, reliable_out, publisher_id, participant_id, UXR_REPLACE|UXR_REUSE);

datawriter_id = uxr_object_id(publisher_index++, UXR_DATAWRITER_ID);
// datawriter_req = uxr_buffer_create_datawriter_xml(&session, reliable_out, datawriter_id, publisher_id, datawriter_xml, UXR_REPLACE|UXR_REUSE);
datawriter_req = uxr_buffer_create_datawriter_bin(&session, reliable_out, datawriter_id, publisher_id, topic_id, reliable_qos, UXR_REPLACE|UXR_REUSE);

// Send create entities message and wait its status
uint8_t status[4];
uint16_t requests[4] = {participant_req, topic_req, publisher_req, datawriter_req};

if(!uxr_run_session_until_all_status(&session, 1000, requests, status, 4))
{
    printf("Error at create entities: participant: %i topic: %i publisher: %i darawriter: %i\n", status[0], status[1], status[2], status[3]);
    return 1;
}
return 0;

}

uint8_t msg_publish(const void * pmsg) { uint32_t topic_size = sensor_msgs_msg_Imu_size_of_topic(pmsg, 0); uxr_prepare_output_stream(&session, reliable_out, datawriter_id, &mb, topic_size); sensor_msgs_msg_Imu_serialize_topic(&mb, pmsg);

// Reliable QoS
if(uxr_run_session_until_confirm_delivery(&session, 100))
    return 0;
else
    return 1;

}

uint8_t create_publisher_imu() { memcpy(msg_imu.header.frame_id,"base_link",9); msg_imu.orientation_covariance[0] = -1.0; msg_imu.orientation_covariance[4] = -1.0; msg_imu.orientation_covariance[8] = -1.0; return create_publisher("rt/imu/data", "sensormsgs::dds::Imu_"); }


# Trials

* use `uxr_buffer_create_xxxxxx_xml()` function instead but don't solve this problem.

# PS

* I've checked `rmw_xrcedds`, I copy the operation in it but still not working. I don't want to use it because it need too much ros2-related building environment tools like ament-cmake which seem weird in MCU project building.
* I've checked `PX4-Autopilot`, but it use template to generate related code and I don't learn to compile it.
pablogs9 commented 8 months ago

Your XRCE Client and XRCE Agent are working properly because the agent is publishing to DDS, as you can see in the log [** <<DDS>> **].

This is likely related to a:

  1. QoS matching problem. What is the output of ros2 topic info /your_topic -v?
  2. Serialization problem. Can you attach your type support?
Pansamic commented 8 months ago

1. QoS matching problem

The output of ros2 topic info /imu/data -v(using Micro-XRCE-DDS):

❯ ros2 topic info /imu/data -v
Type: sensor_msgs/Imu

Publisher count: 1

Node name: nav_mecanum_driver
Node namespace: /
Topic type: sensor_msgs/Imu
Endpoint type: PUBLISHER
GID: 01.0f.88.f0.36.21.c5.a0.04.00.00.00.00.00.01.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0

Output of ros2 topic info /imu/data -v(using Micro-ROS static library). This is the version that its message can be received by my ros2 nodes and can be echoed so it's my reference.

❯ ros2 topic info /imu/data -v
Type: sensor_msgs/msg/Imu

Publisher count: 1

Node name: nav_mecanum_driver
Node namespace: /
Topic type: sensor_msgs/msg/Imu
Endpoint type: PUBLISHER
GID: 01.0f.88.f0.36.21.c5.a0.03.00.00.00.00.00.0c.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0

As you can see, Topic type is not the same. QoS is the same.

2. Serialization problem

I've noticed something about rosidl_typesupport_microxrcedds but I haven't use that. Is it essential? If so, how to use it? I find so much repos to find the dataType of Micro-XRCE-DDS of ROS2 message, like PX4-Autopilot, ros2xrcedds, rmw_xrcedds but I failed.

pablogs9 commented 8 months ago

The type name shall be fixed in order to have a proper DDS/ROS2 communication

Pansamic commented 8 months ago

thanks, it works.