Bind a Fast RTPS publisher with a ROS 2 subscriber fails #251
Comments
|
@nuclearsandwich @dirk-thomas any input you can give here? Thanks! |
|
In the code snippet creating the publisher you are setting a partition. In Ardent the namespace part of a topic was mapped to DDS partitions. In Bouncy as well as Crystal that is not the case anymore. Partitions are not used anything - instead the namespace stays part of the topic name. Not sure if the following is the best reference but it should give you a starting point: ros2/ros2#476 |
|
@dirk-thomas I just tried to remove the partitions and the result is the same. Here's an example of a generated message Publisher: #include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/publisher/Publisher.h>
#include <fastrtps/attributes/PublisherAttributes.h>
#include <fastrtps/Domain.h>
#include <fastrtps/utils/eClock.h>
#include "SensorCombined_Publisher.h"
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;
SensorCombined_Publisher::SensorCombined_Publisher() : mp_participant(nullptr), mp_publisher(nullptr) {}
SensorCombined_Publisher::~SensorCombined_Publisher() { Domain::removeParticipant(mp_participant);}
bool SensorCombined_Publisher::init()
{
// Create RTPSParticipant
ParticipantAttributes PParam;
PParam.rtps.builtin.domainId = 0;
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.setName("SensorCombined_publisher"); //You can put here the name you want
mp_participant = Domain::createParticipant(PParam);
if(mp_participant == nullptr)
return false;
//Register the type
Domain::registerType(mp_participant, (TopicDataType*) &myType);
// Create Publisher
PublisherAttributes Wparam;
Wparam.topic.topicKind = NO_KEY;
Wparam.topic.topicDataType = myType.getName(); //This type MUST be registered
Wparam.topic.topicName = "SensorCombined_topic";
mp_publisher = Domain::createPublisher(mp_participant, Wparam, (PublisherListener*) &m_listener);
if(mp_publisher == nullptr)
return false;
//std::cout << "Publisher created, waiting for Subscribers." << std::endl;
return true;
}
void SensorCombined_Publisher::PubListener::onPublicationMatched(Publisher* pub, MatchingInfo& info)
{
if (info.status == MATCHED_MATCHING)
{
n_matched++;
std::cout << "Publisher matched" << std::endl;
}
else
{
n_matched--;
std::cout << "Publisher unmatched" << std::endl;
}
}
void SensorCombined_Publisher::run()
{
while(m_listener.n_matched == 0)
{
eClock::my_sleep(250); // Sleep 250 ms
}
// Publication code
px4_ros_com::msg::dds_::SensorCombined_ st;
/* Initialize your structure here */
int msgsent = 0;
char ch = 'y';
do
{
if(ch == 'y')
{
mp_publisher->write(&st); ++msgsent;
std::cout << "Sending sample, count=" << msgsent << ", send another sample?(y-yes,n-stop): ";
}
else if(ch == 'n')
{
std::cout << "Stopping execution " << std::endl;
break;
}
else
{
std::cout << "Command " << ch << " not recognized, please enter \"y/n\":";
}
}while(std::cin >> ch);
}
void SensorCombined_Publisher::publish(px4_ros_com::msg::dds_::SensorCombined_* st)
{
mp_publisher->write(st);
}and here's the generated #include <fastcdr/FastBuffer.h>
#include <fastcdr/Cdr.h>
#include "SensorCombined_PubSubTypes.h"
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;
namespace px4_ros_com
{
namespace msg
{
namespace dds_
{
SensorCombined_PubSubType::SensorCombined_PubSubType() {
setName("px4_ros_com::msg::dds_::SensorCombined_");
m_typeSize = (uint32_t)SensorCombined_::getMaxCdrSerializedSize() + 4 /*encapsulation*/;
m_isGetKeyDefined = SensorCombined_::isKeyDefined();
m_keyBuffer = (unsigned char*)malloc(SensorCombined_::getKeyMaxCdrSerializedSize()>16 ? SensorCombined_::getKeyMaxCdrSerializedSize() : 16);
}
SensorCombined_PubSubType::~SensorCombined_PubSubType() {
if(m_keyBuffer!=nullptr)
free(m_keyBuffer);
}
bool SensorCombined_PubSubType::serialize(void *data, SerializedPayload_t *payload) {
SensorCombined_ *p_type = (SensorCombined_*) data;
eprosima::fastcdr::FastBuffer fastbuffer((char*) payload->data, payload->max_size); // Object that manages the raw buffer.
eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
eprosima::fastcdr::Cdr::DDS_CDR); // Object that serializes the data.
payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
// Serialize encapsulation
ser.serialize_encapsulation();
try
{
p_type->serialize(ser); // Serialize the object:
}
catch(eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
{
return false;
}
payload->length = (uint32_t)ser.getSerializedDataLength(); //Get the serialized length
return true;
}
bool SensorCombined_PubSubType::deserialize(SerializedPayload_t* payload, void* data) {
SensorCombined_* p_type = (SensorCombined_*) data; //Convert DATA to pointer of your type
eprosima::fastcdr::FastBuffer fastbuffer((char*)payload->data, payload->length); // Object that manages the raw buffer.
eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
eprosima::fastcdr::Cdr::DDS_CDR); // Object that deserializes the data.
// Deserialize encapsulation.
deser.read_encapsulation();
payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
try
{
p_type->deserialize(deser); //Deserialize the object:
}
catch(eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
{
return false;
}
return true;
}
std::function<uint32_t()> SensorCombined_PubSubType::getSerializedSizeProvider(void* data) {
return [data]() -> uint32_t
{
return (uint32_t)type::getCdrSerializedSize(*static_cast<SensorCombined_*>(data)) + 4 /*encapsulation*/;
};
}
void* SensorCombined_PubSubType::createData() {
return (void*)new SensorCombined_();
}
void SensorCombined_PubSubType::deleteData(void* data) {
delete((SensorCombined_*)data);
}
bool SensorCombined_PubSubType::getKey(void *data, InstanceHandle_t* handle) {
if(!m_isGetKeyDefined)
return false;
SensorCombined_* p_type = (SensorCombined_*) data;
eprosima::fastcdr::FastBuffer fastbuffer((char*)m_keyBuffer,SensorCombined_::getKeyMaxCdrSerializedSize()); // Object that manages the raw buffer.
eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS); // Object that serializes the data.
p_type->serializeKey(ser);
if(SensorCombined_::getKeyMaxCdrSerializedSize()>16) {
m_md5.init();
m_md5.update(m_keyBuffer,(unsigned int)ser.getSerializedDataLength());
m_md5.finalize();
for(uint8_t i = 0;i<16;++i) {
handle->value[i] = m_md5.digest[i];
}
}
else {
for(uint8_t i = 0;i<16;++i) {
handle->value[i] = m_keyBuffer[i];
}
}
return true;
}
} //End of namespace dds_
} //End of namespace msg
} //End of namespace px4_ros_comThe reference you gave me seems to address the removal of the partition, but I don't know what else should I aim to here. Any other tip? Thanks! |
|
Your snippet uses the topic name I would suggest to either debug the subscriber on the ROS 2 side to see what actual values are being used (so that your code can match all names and QoS settings) or to use a tool to introspect the RTPS packets which will show how each side tried to discover the publisher / subscriber and find the difference between them that way. |
|
The listener code is exactly the same for Ardent and for Bouncy, though just works on Ardent: https://github.com/PX4/px4_ros_com/blob/master/src/listeners/sensor_combined_listener.cpp. The topic naming matches. Anything abnormal you can see on it? |
|
If you have access to the RTI Admin Console I would recommend that. Otherwise Wireshark is probably your best bet. |
|
Thanks I will try that. Though, can you specify what were the more relevant changes on the rmw_fastrtps implementation from Ardent to Bouncy? I mean, what differences should I expect to find besides the partition? Anything on the QoS as well? |
|
You can find all changes between Bouncy and Ardent here: ardent...bouncy Beside the already mentioned #192 nothing sticks out to me. As mentioned before I would suggest to double check your code. Before you used |
I am not sure of the actual meaning of this. Does that mean I need to include the partition but pass it with an empty string? |
|
Your topic name needs to be something like this |
|
You should be able to see the exact names with this script: #!/usr/bin/env python3
import pprint
import rclpy
rclpy.init()
n = rclpy.create_node('list_no_demangle')
pprint.pprint(n.get_topic_names_and_types(True))For example with talker I get this: Note this: |
|
This is fantastic! Thank you very much @dirk-thomas (and @wjwwood for the script)! I just added the |
|
@dirk-thomas, @wjwwood I am facing again the same issue regarding the binding between a Fast-RTPS publisher and a ROS subscriber, but this time with ROS2 Crystal. bool SensorCombined_Publisher::init()
{
// Create RTPSParticipant
ParticipantAttributes PParam;
PParam.rtps.builtin.domainId = 0;
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.setName("SensorCombined_publisher"); //You can put here the name you want
mp_participant = Domain::createParticipant(PParam);
if(mp_participant == nullptr)
return false;
//Register the type
Domain::registerType(mp_participant, (TopicDataType*) &myType);
// Create Publisher
PublisherAttributes Wparam;
Wparam.topic.topicKind = NO_KEY;
Wparam.topic.topicDataType = myType.getName(); //This type MUST be registered
Wparam.topic.topicName = "rt/SensorCombined_topic";
mp_publisher = Domain::createPublisher(mp_participant, Wparam, (PublisherListener*) &m_listener);
if(mp_publisher == nullptr)
return false;
//std::cout << "Publisher created, waiting for Subscribers." << std::endl;
return true;
}And the respective ROS2 subscriber: #include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/sensor_combined.hpp>
/**
* @brief Sensor Combined uORB topic data callback
*/
class SensorCombinedListener : public rclcpp::Node
{
public:
explicit SensorCombinedListener() : Node("sensor_combined_listener") {
subscription_ = this->create_subscription<px4_msgs::msg::SensorCombined>(
"SensorCombined_topic",
[this](const px4_msgs::msg::SensorCombined::UniquePtr msg) {
std::cout << "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n";
std::cout << "RECEIVED DATA ON SENSOR COMBINED" << std::endl;
std::cout << "================================" << std::endl;
std::cout << "ts: " << msg->timestamp << std::endl;
std::cout << "gyro_rad[0]: " << msg->gyro_rad[0] << std::endl;
std::cout << "gyro_rad[1]: " << msg->gyro_rad[1] << std::endl;
std::cout << "gyro_rad[2]: " << msg->gyro_rad[2] << std::endl;
std::cout << "gyro_integral_dt: " << msg->gyro_integral_dt << std::endl;
std::cout << "accelerometer_timestamp_relative: " << msg->accelerometer_timestamp_relative << std::endl;
std::cout << "accelerometer_m_s2[0]: " << msg->accelerometer_m_s2[0] << std::endl;
std::cout << "accelerometer_m_s2[1]: " << msg->accelerometer_m_s2[1] << std::endl;
std::cout << "accelerometer_m_s2[2]: " << msg->accelerometer_m_s2[2] << std::endl;
std::cout << "accelerometer_integral_dt: " << msg->accelerometer_integral_dt << std::endl;
});
}
private:
rclcpp::Subscription<px4_msgs::msg::SensorCombined>::SharedPtr subscription_;
};
int main(int argc, char *argv[])
{
std::cout << "Starting sensor_combined listener node..." << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<SensorCombinedListener>());
rclcpp::shutdown();
return 0;
}Is there any QoS configuration, or something else, that I am missing in comparison with the working setup on Bouncy? Thanks in advance! |
|
@wjwwood btw, using your script I get the following output: [('rq/list_no_demangle/describe_parametersRequest',
['rcl_interfaces::srv::dds_::DescribeParameters_Request_']),
('rq/list_no_demangle/get_parameter_typesRequest',
['rcl_interfaces::srv::dds_::GetParameterTypes_Request_']),
('rq/list_no_demangle/get_parametersRequest',
['rcl_interfaces::srv::dds_::GetParameters_Request_']),
('rq/list_no_demangle/list_parametersRequest',
['rcl_interfaces::srv::dds_::ListParameters_Request_']),
('rq/list_no_demangle/set_parametersRequest',
['rcl_interfaces::srv::dds_::SetParameters_Request_']),
('rq/list_no_demangle/set_parameters_atomicallyRequest',
['rcl_interfaces::srv::dds_::SetParametersAtomically_Request_']),
('rq/sensor_combined_listener/describe_parametersRequest',
['rcl_interfaces::srv::dds_::DescribeParameters_Request_']),
('rq/sensor_combined_listener/get_parameter_typesRequest',
['rcl_interfaces::srv::dds_::GetParameterTypes_Request_']),
('rq/sensor_combined_listener/get_parametersRequest',
['rcl_interfaces::srv::dds_::GetParameters_Request_']),
('rq/sensor_combined_listener/list_parametersRequest',
['rcl_interfaces::srv::dds_::ListParameters_Request_']),
('rq/sensor_combined_listener/set_parametersRequest',
['rcl_interfaces::srv::dds_::SetParameters_Request_']),
('rq/sensor_combined_listener/set_parameters_atomicallyRequest',
['rcl_interfaces::srv::dds_::SetParametersAtomically_Request_']),
('rr/list_no_demangle/describe_parametersReply',
['rcl_interfaces::srv::dds_::DescribeParameters_Response_']),
('rr/list_no_demangle/get_parameter_typesReply',
['rcl_interfaces::srv::dds_::GetParameterTypes_Response_']),
('rr/list_no_demangle/get_parametersReply',
['rcl_interfaces::srv::dds_::GetParameters_Response_']),
('rr/list_no_demangle/list_parametersReply',
['rcl_interfaces::srv::dds_::ListParameters_Response_']),
('rr/list_no_demangle/set_parametersReply',
['rcl_interfaces::srv::dds_::SetParameters_Response_']),
('rr/list_no_demangle/set_parameters_atomicallyReply',
['rcl_interfaces::srv::dds_::SetParametersAtomically_Response_']),
('rr/sensor_combined_listener/describe_parametersReply',
['rcl_interfaces::srv::dds_::DescribeParameters_Response_']),
('rr/sensor_combined_listener/get_parameter_typesReply',
['rcl_interfaces::srv::dds_::GetParameterTypes_Response_']),
('rr/sensor_combined_listener/get_parametersReply',
['rcl_interfaces::srv::dds_::GetParameters_Response_']),
('rr/sensor_combined_listener/list_parametersReply',
['rcl_interfaces::srv::dds_::ListParameters_Response_']),
('rr/sensor_combined_listener/set_parametersReply',
['rcl_interfaces::srv::dds_::SetParameters_Response_']),
('rr/sensor_combined_listener/set_parameters_atomicallyReply',
['rcl_interfaces::srv::dds_::SetParametersAtomically_Response_']),
('rt/AdcReport_topic', ['px4_ros_com::msg::dds_::AdcReport_']),
('rt/Airspeed_topic', ['px4_ros_com::msg::dds_::Airspeed_']),
('rt/BatteryStatus_topic', ['px4_ros_com::msg::dds_::BatteryStatus_']),
('rt/CameraCapture_topic', ['px4_ros_com::msg::dds_::CameraCapture_']),
('rt/CameraTrigger_topic', ['px4_ros_com::msg::dds_::CameraTrigger_']),
('rt/CollisionReport_topic', ['px4_ros_com::msg::dds_::CollisionReport_']),
('rt/Cpuload_topic', ['px4_ros_com::msg::dds_::Cpuload_']),
('rt/DebugArray_topic', ['px4_ros_com::msg::dds_::DebugArray_']),
('rt/DebugKeyValue_topic', ['px4_ros_com::msg::dds_::DebugKeyValue_']),
('rt/DebugValue_topic', ['px4_ros_com::msg::dds_::DebugValue_']),
('rt/DebugVect_topic', ['px4_ros_com::msg::dds_::DebugVect_']),
('rt/EstimatorStatus_topic', ['px4_ros_com::msg::dds_::EstimatorStatus_']),
('rt/HomePosition_topic', ['px4_ros_com::msg::dds_::HomePosition_']),
('rt/IridiumsbdStatus_topic', ['px4_ros_com::msg::dds_::IridiumsbdStatus_']),
('rt/ObstacleDistance_topic', ['px4_ros_com::msg::dds_::ObstacleDistance_']),
('rt/PositionSetpointTriplet_topic',
['px4_ros_com::msg::dds_::PositionSetpointTriplet_']),
('rt/SensorCombined_topic', ['px4_msgs::msg::dds_::SensorCombined_']),
('rt/SensorSelection_topic', ['px4_ros_com::msg::dds_::SensorSelection_']),
('rt/TrajectoryWaypoint_topic',
['px4_ros_com::msg::dds_::TrajectoryWaypoint_']),
('rt/VehicleAttitude_topic', ['px4_ros_com::msg::dds_::VehicleAttitude_']),
('rt/VehicleOdometry_topic', ['px4_ros_com::msg::dds_::VehicleOdometry_']),
('rt/VehicleTrajectoryWaypoint_topic',
['px4_ros_com::msg::dds_::VehicleTrajectoryWaypoint_']),
('rt/WindEstimate_topic', ['px4_ros_com::msg::dds_::WindEstimate_']),
('rt/parameter_events', ['rcl_interfaces::msg::dds_::ParameterEvent_']),
('rt/rosout', ['rcl_interfaces::msg::dds_::Log_'])] |
|
I see only |
@wjwwood thanks for the feedback. What other settings are these that made this incompatible from ROS Bouncy to ROS Crystal? |
|
There were none of which I am aware, that's why I encouraged you to double check that your network is working properly. Something may have changed, but not that I can remember. |
How can that be verified? There are no changes on the agent code or the ROS2 subscriber that would justify this. |
|
@richiware maybe you can give a hint here? Thanks! |
|
Can you provide me a Wireshark capture where FastRTPS publisher and ROS2 subscriber are involved? It will be helpful for me. |
|
@richiware @richiprosima here it goes: https://drive.google.com/open?id=1bE9j-zO-2_4BcieeDeuxlY2Q9qOSZhPS. On this example, you have the microRTPS agent publishing multiple types of messages, but the ones in specific that matter are the SensorCombined ones, for which the |
|
@richiware can you help me with this? Thank you |
|
I miss some submessages. Can you capture also the localhost interface? |
I would I set a similar config on the micrortps_agent participant? |
|
I don't know if not adding that information implies no communication with a ROS2 node. Maybe @wjwwood could answer that. |
|
No, it will not prevent communication, but it is used for things like |
Well, then I am kinda stuck on what to actually investigate. |
|
@wjwwood @richiware can you please suggest on other things to verify so to understand why there's no bind? I really appreciate your inputs. Thanks in advance |
|
@TSC21 did you give @richiprosima the "capture data with localhost"? I don't have any more ideas, I haven't worked on either Fast-RTPS or rmw_fastrtps_cpp recently, but eProsima did refactor the rmw implementation last release (e.g. #203), so presumably they know if anything significant changed. After that, communication works for us, and all the code that configures Fast-RTPS for us is here: https://github.com/ros2/rmw_fastrtps/ So you can look at that yourself to see if we're doing anything strange, but I don't think we are. |
I was wondering what in specific are we looking for on
I already tried that myself but I nothing comes obvious to my eyes. The PR you are showing me has the code of the release I was already using in Bouncy, and I suppose now in Crystal. What I wanted to understand if it there was some obvious change on the participant or the publisher/subscribers configurations that need to be taken into account so there's a bind between a pure Fast-RTPS publisher and a ROS2 subscriber. |
|
@TSC21, as you are executing both applications in the same machine, several RTPS messages are sent through localhost, and I couldn't see them in your previous Wireshark capture. |
@richiware understood. Here it goes: https://drive.google.com/open?id=1WctMEIdQrokgFZrfBVTnYidszuZwCMNb. Thanks in advance! |
|
@richiprosima @richiware any input? Thanks! |
|
Your Publisher and ROS2 Subscriber are using the same topic name: "rt/SensorCombined_topic". But the type name differs. Your Publisher is using "px4_ros_com::msg::dds_::SensorCombined_" and ROS2 Subscriber is using "px4_msgs::msg::dds_::SensorCombined_". This is the reason they are not matching. |
|
Oh right!! That makes much more sense now! Can you please tell me how did yoi verify that on the Wireshark? After some inspection I didn't find that info. Thanks!! |
|
Publisher info ( |
|
@richiware thank you very much for your input! I was able to solve this by setting the same userspace type for the publisher and the subscriber. It was an issue introduced after a big package restructure. Again, thanks! |
|
@richiprosima looks like for each release I always stump into the same issue. This time with ROS 2 Dashing. Using Fast-RTPS 1.8.2. But now, trying to analyse the Wireshark capture, I didn't find any clues: https://drive.google.com/open?id=1QTx6xjRQZ1Rh1F-sibek40AvvSsF3hw6. Topic names look the same, types as well. I don't find Can you help here? Thank you! |
TSC21
changed the title
Bug report
Required Info:
bouncySteps to reproduce issue
Follow the steps set in:
The above will generate sources and headers for a micro RTPS agent which will publish and subscribe to/from data coming from the ROS2 Fast RTPS layer.
Expected behavior
With Ardent, the agent is able to bind with ROS2 publishers/subscribers - a
Publisher matchedor aSubscriber matchedmessage appears on the console output of the agent.UPDATE: It's expected to work on ROS2 Crystal as well.
Actual behavior
Currently it only appears to work using Ardent. UPDATE: Works on Bouncy as well. Doesn't work on Crystal.
Additional information
It's not obvious to me if this is related to the current implementation of the
rmw_fastrtpsto Bouncy, but it was proven that this problem only occurs while using Bouncy. The bind happens when using Ardent.UPDATE: Works now in Bouncy. Doesn't work on Crystal.
The current template used for the micro-RTPS agent Publisher is https://github.com/PX4/px4_ros_com/blob/master/msg/templates/Publisher.cpp.template#L76-L104. Again, this template works well with Ardent and allows binding the agent and the ROS2 nodes.
I tried to research the current implementation of Fast RTPS publishers and subscribers inside
rmw_fastrtpsbut could not find any relevant differences that would limit the functionality.Any tips to solve this are welcomed!
Thanks in advance!
The text was updated successfully, but these errors were encountered: