Feature/graph manager (#38)

* Initial changes * Working state * Working state * Working state * Working state * Working state * Working state * Added graph listener * Updates * Testing name demangling * Graph manager: reworked * Remove debug trace * Treat errors when freeing rcutils resources * Improve logics for discovering entities * Working state: Micro-ROS graph manager * Fix micro_ros_graph_info typesupport type name * Remove double CMakeLists comment * Remove package.xml commented block * Add missing include * Added CI * Remove GUID_t param from callbacks signature * Add services names and types to micro-ROS graph topic * Differenciate between service server and clients * remove debug traces Co-authored-by: Pablo Garrido <pablogs9@gmail.com>
2026-09-06 18:05:17 +02:00 · 2020-11-26 07:44:52 +01:00
parent dce3c25a33
commit 23bcea8efb
18 changed files with 2090 additions and 15 deletions
@@ -0,0 +1,166 @@
+// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef _UROS_AGENT_AGENT_CPP
+#define _UROS_AGENT_AGENT_CPP
+
+#include <agent/Agent.hpp>
+
+namespace uros {
+namespace agent {
+
+Agent::Agent()
+    : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance())
+    , graph_manager_(std::make_unique<graph_manager::GraphManager>())
+{
+    /**
+     * Add CREATE_PARTICIPANT callback.
+     */
+    std::function<void (
+        const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
+        ([&](
+            const eprosima::fastdds::dds::DomainParticipant* participant) -> void
+        {
+            graph_manager_->add_participant(participant);
+        });
+    xrce_dds_agent_instance_.add_middleware_callback(
+        eprosima::uxr::Middleware::Kind::FASTDDS,
+        eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT,
+        std::move(on_create_participant));
+
+    /**
+     * Add REMOVE_PARTICIPANT callback.
+     */
+    std::function<void (
+        const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
+        ([&](
+            const eprosima::fastdds::dds::DomainParticipant* participant) -> void
+        {
+            graph_manager_->remove_participant(participant->guid());
+        });
+    xrce_dds_agent_instance_.add_middleware_callback(
+        eprosima::uxr::Middleware::Kind::FASTDDS,
+        eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT,
+        std::move(on_delete_participant));
+
+    /**
+     * Add CREATE_DATAWRITER callback.
+     */
+    std::function<void (
+        const eprosima::fastdds::dds::DomainParticipant *,
+        const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
+        ([&](
+            const eprosima::fastdds::dds::DomainParticipant* participant,
+            const eprosima::fastdds::dds::DataWriter* datawriter) -> void
+        {
+            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                datawriter->get_instance_handle();
+            const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
+                iHandle2GUID(instance_handle);
+            graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
+            graph_manager_->associate_entity(
+                datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
+        });
+    xrce_dds_agent_instance_.add_middleware_callback(
+        eprosima::uxr::Middleware::Kind::FASTDDS,
+        eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER,
+        std::move(on_create_datawriter));
+
+    /**
+     * Add DELETE_DATAWRITER callback.
+     */
+    std::function<void (
+        const eprosima::fastdds::dds::DomainParticipant *,
+        const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
+        ([&](
+            const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
+            const eprosima::fastdds::dds::DataWriter* datawriter) -> void
+        {
+            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                datawriter->get_instance_handle();
+            const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
+                eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
+            graph_manager_->remove_datawriter(datawriter_guid);
+        });
+
+    xrce_dds_agent_instance_.add_middleware_callback(
+        eprosima::uxr::Middleware::Kind::FASTDDS,
+        eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER,
+        std::move(on_delete_datawriter));
+
+    /**
+     * Add CREATE_DATAREADER callback.
+     */
+    std::function<void (
+        const eprosima::fastdds::dds::DomainParticipant *,
+        const eprosima::fastdds::dds::DataReader*)> on_create_datareader
+        ([&](
+            const eprosima::fastdds::dds::DomainParticipant* participant,
+            const eprosima::fastdds::dds::DataReader* datareader) -> void
+        {
+            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                datareader->get_instance_handle();
+            const eprosima::fastrtps::rtps::GUID_t datareader_guid =
+                eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
+            graph_manager_->add_datareader(datareader_guid, participant, datareader);
+            graph_manager_->associate_entity(
+                datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
+        });
+    xrce_dds_agent_instance_.add_middleware_callback(
+        eprosima::uxr::Middleware::Kind::FASTDDS,
+        eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER,
+        std::move(on_create_datareader));
+
+    /**
+     * Add DELETE_DATAREADER callback.
+     */
+    std::function<void (
+        const eprosima::fastdds::dds::DomainParticipant *,
+        const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
+        ([&](
+            const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
+            const eprosima::fastdds::dds::DataReader* datareader) -> void
+        {
+            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                datareader->get_instance_handle();
+            const eprosima::fastrtps::rtps::GUID_t datareader_guid =
+                eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
+            graph_manager_->remove_datareader(datareader_guid);
+        });
+
+    xrce_dds_agent_instance_.add_middleware_callback(
+        eprosima::uxr::Middleware::Kind::FASTDDS,
+        eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
+        std::move(on_delete_datareader));
+}
+
+bool Agent::create(
+        int argc,
+        char** argv)
+{
+    return xrce_dds_agent_instance_.create(argc, argv);
+}
+
+void Agent::run()
+{
+    return xrce_dds_agent_instance_.run();
+}
+
+}  // namespace agent
+}  // namespace uros
+#endif  // _UROS_AGENT_AGENT_CPP