diff --git a/micro_ros_agent/CMakeLists.txt b/micro_ros_agent/CMakeLists.txt index 1ae84a1..bed0de4 100644 --- a/micro_ros_agent/CMakeLists.txt +++ b/micro_ros_agent/CMakeLists.txt @@ -38,7 +38,7 @@ find_package(fastdds REQUIRED) find_package(rmw_dds_common REQUIRED) find_package(rmw REQUIRED) find_package(rcutils REQUIRED) -find_package(rmw_fastrtps_shared_cpp REQUIRED) +find_package(rcpputils REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -65,9 +65,9 @@ target_include_directories(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${micro_ros_msgs_TARGETS} ${rmw_dds_common_TARGETS} + rcpputils::rcpputils rmw::rmw rmw_dds_common::rmw_dds_common_library - rmw_fastrtps_shared_cpp::rmw_fastrtps_shared_cpp rosidl_runtime_cpp::rosidl_runtime_cpp rosidl_typesupport_cpp::rosidl_typesupport_cpp rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp diff --git a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp index 774b459..14542d3 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -37,10 +37,9 @@ #include "rmw/types.h" #include "rmw/names_and_types.h" +#include "rmw/qos_profiles.h" #include "rmw/impl/cpp/key_value.hpp" #include "rmw_dds_common/graph_cache.hpp" -#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rcutils/types.h" #include "rcutils/types/string_array.h" diff --git a/micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp b/micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp index 555bcbf..02f049d 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp @@ -29,8 +29,6 @@ #include "rmw/types.h" #include "rmw/impl/cpp/key_value.hpp" #include "rmw_dds_common/graph_cache.hpp" -#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rcutils/types.h" diff --git a/micro_ros_agent/package.xml b/micro_ros_agent/package.xml index 3264ac4..0a319e1 100644 --- a/micro_ros_agent/package.xml +++ b/micro_ros_agent/package.xml @@ -17,7 +17,7 @@ rmw rcutils - rmw_fastrtps_shared_cpp + rcpputils rmw_dds_common micro_ros_msgs microxrcedds_agent diff --git a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp index b74600c..88abf62 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,23 @@ namespace uros { namespace agent { namespace graph_manager { +// Builds a GID the way rmw_fastrtps_cpp does, the 16-byte GUID copied as is +// Defined here since rmw_fastrtps_shared_cpp, which provided it, was removed upstream +// (ros2/rmw_fastrtps#923) +rmw_gid_t create_rmw_gid( + const char* identifier, + const eprosima::fastdds::rtps::GUID_t& guid) +{ + static_assert( + sizeof(guid) == sizeof(guid.guidPrefix.value) + sizeof(guid.entityId.value) && + sizeof(guid) <= RMW_GID_STORAGE_SIZE, + "Fast DDS GUID_t must be a packed prefix + entity id that fits in rmw_gid_t"); + rmw_gid_t rmw_gid = {}; + rmw_gid.implementation_identifier = identifier; + std::memcpy(rmw_gid.data, &guid, sizeof(guid)); + return rmw_gid; +} + GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) : domain_id_(domain_id) , graph_changed_(false) @@ -307,7 +325,7 @@ void GraphManager::add_participant( const std::string& enclave) { const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos(); - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); + const rmw_gid_t gid = create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); graphCache_.add_participant(gid, enclave); @@ -339,7 +357,7 @@ void GraphManager::remove_participant( const eprosima::fastdds::dds::DomainParticipant* participant, bool from_microros) { - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); + const rmw_gid_t gid = create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); graphCache_.remove_participant(gid); if (from_microros) @@ -371,9 +389,9 @@ void GraphManager::add_datawriter( const eprosima::fastdds::rtps::GUID_t& participant_guid, const eprosima::fastdds::dds::DataWriterQos& writer_qos) { - const rmw_gid_t datawriter_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t datawriter_gid = create_rmw_gid( "rmw_fastrtps_cpp", datawriter_guid); - const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t participant_gid = create_rmw_gid( "rmw_fastrtps_cpp", participant_guid); const rmw_qos_profile_t qos_profile = fastdds_qos_to_rmw_qos(writer_qos); @@ -384,7 +402,7 @@ void GraphManager::add_datawriter( void GraphManager::remove_datawriter( const eprosima::fastdds::rtps::GUID_t& datawriter_guid) { - const rmw_gid_t datawriter_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t datawriter_gid = create_rmw_gid( "rmw_fastrtps_cpp", datawriter_guid); graphCache_.remove_entity(datawriter_gid, false); @@ -408,9 +426,9 @@ void GraphManager::add_datareader( const eprosima::fastdds::rtps::GUID_t& participant_guid, const eprosima::fastdds::dds::DataReaderQos& reader_qos) { - const rmw_gid_t datareader_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t datareader_gid = create_rmw_gid( "rmw_fastrtps_cpp", datareader_guid); - const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t participant_gid = create_rmw_gid( "rmw_fastrtps_cpp", participant_guid); const rmw_qos_profile_t qos_profile = fastdds_qos_to_rmw_qos(reader_qos); @@ -426,7 +444,7 @@ void GraphManager::add_datareader( void GraphManager::remove_datareader( const eprosima::fastdds::rtps::GUID_t& datareader_guid) { - const rmw_gid_t datareader_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t datareader_gid = create_rmw_gid( "rmw_fastrtps_cpp", datareader_guid); graphCache_.remove_entity(datareader_gid, true); @@ -437,9 +455,9 @@ void GraphManager::associate_entity( const eprosima::fastdds::dds::DomainParticipant* participant, const dds::xrce::ObjectKind& entity_kind) { - const rmw_gid_t entity_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t entity_gid = create_rmw_gid( "rmw_fastrtps_cpp", entity_guid); - const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + const rmw_gid_t participant_gid = create_rmw_gid( "rmw_fastrtps_cpp", participant->guid()); eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos(); @@ -629,14 +647,14 @@ void GraphManager::ParticipantListener::on_participant_discovery( const std::string enclave = std::string(name_found->second.begin(), name_found->second.end()); - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", info.guid); + const rmw_gid_t gid = create_rmw_gid("rmw_fastrtps_cpp", info.guid); graphManager_from_->get_graph_cache().add_participant(gid, enclave); break; } case eprosima::fastdds::rtps::ParticipantDiscoveryStatus::REMOVED_PARTICIPANT: case eprosima::fastdds::rtps::ParticipantDiscoveryStatus::DROPPED_PARTICIPANT: { - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", info.guid); + const rmw_gid_t gid = create_rmw_gid("rmw_fastrtps_cpp", info.guid); graphManager_from_->get_graph_cache().remove_participant(gid); break; } diff --git a/micro_ros_agent/src/agent/utils/demangle.cpp b/micro_ros_agent/src/agent/utils/demangle.cpp index de78aef..f3128ad 100644 --- a/micro_ros_agent/src/agent/utils/demangle.cpp +++ b/micro_ros_agent/src/agent/utils/demangle.cpp @@ -23,14 +23,44 @@ #include #include -#include - #include namespace uros { namespace agent { namespace utils { +// ROS 2 name prefixes on DDS topics. Defined here since rmw_fastrtps_shared_cpp, +// which provided them, was removed upstream (ros2/rmw_fastrtps#923) +const char* const ros_topic_prefix = "rt"; +const char* const ros_service_requester_prefix = "rq"; +const char* const ros_service_response_prefix = "rr"; + +// Returns name without "", or "" if name does not start with "/". +// Defined here for the same reason as the prefixes above +std::string _resolve_prefix( + const std::string& name, + const std::string& prefix) +{ + return (name.rfind(prefix + "/", 0) == 0) ? name.substr(prefix.length()) : ""; +} + +// Returns topic_name without its ROS 2 prefix, if it has one. +// Defined here for the same reason as the prefixes above +std::string _strip_ros_prefix_if_exists( + const std::string& topic_name) +{ + for (const char* prefix : + {ros_topic_prefix, ros_service_requester_prefix, ros_service_response_prefix}) + { + const std::string stripped = _resolve_prefix(topic_name, prefix); + if (!stripped.empty()) + { + return stripped; + } + } + return topic_name; +} + std::string Demangle::demangle_if_ros_topic( const std::string& topic_name) {