Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions micro_ros_agent/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
2 changes: 1 addition & 1 deletion micro_ros_agent/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

<depend>rmw</depend>
<depend>rcutils</depend>
<depend>rmw_fastrtps_shared_cpp</depend>
<depend>rcpputils</depend>
<depend>rmw_dds_common</depend>
<depend>micro_ros_msgs</depend>
<depend>microxrcedds_agent</depend>
Expand Down
42 changes: 30 additions & 12 deletions micro_ros_agent/src/agent/graph_manager/graph_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <fastdds/rtps/participant/ParticipantDiscoveryInfo.hpp>
#include <fastdds/rtps/reader/ReaderDiscoveryStatus.hpp>

#include <cstring>
#include <memory>
#include <string>
#include <utility>
Expand All @@ -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)
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);

Expand All @@ -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);
Expand All @@ -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);

Expand All @@ -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);
Expand All @@ -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();
Expand Down Expand Up @@ -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;
}
Expand Down
34 changes: 32 additions & 2 deletions micro_ros_agent/src/agent/utils/demangle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,14 +23,44 @@
#include <rcutils/logging_macros.h>
#include <rcutils/types.h>

#include <rmw_fastrtps_shared_cpp/namespace_prefix.hpp>

#include <agent/utils/demangle.hpp>

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 "<prefix>", or "" if name does not start with "<prefix>/".
// 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)
{
Expand Down
Loading