Skip to content

Commit a3fe4aa

Browse files
pablogs9Acuadros95
pablogs9
and
Acuadros95
authored
Add multi domain graph manager (#69)
* Add multi domain graph manager * Update * Fix * Fix * Update micro_ros_agent/include/agent/graph_manager/graph_manager.hpp Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com> Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com>
1 parent 003cb38 commit a3fe4aa

File tree

4 files changed

+68
-13
lines changed

4 files changed

+68
-13
lines changed

‎micro_ros_agent/include/agent/Agent.hpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,9 @@ class Agent
4141
private:
4242

4343
eprosima::uxr::AgentInstance& xrce_dds_agent_instance_;
44-
std::unique_ptr<graph_manager::GraphManager> graph_manager_;
44+
std::map<eprosima::fastdds::dds::DomainId_t, std::shared_ptr<graph_manager::GraphManager>> graph_manager_map_;
45+
46+
std::shared_ptr<graph_manager::GraphManager> find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id);
4547
};
4648

4749
} // namespace agent

‎micro_ros_agent/include/agent/graph_manager/graph_manager.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,7 @@ class GraphManager
7575
/**
7676
* @brief Default constructor.
7777
*/
78-
GraphManager();
78+
GraphManager(eprosima::fastdds::dds::DomainId_t domain_id);
7979

8080
/**
8181
* @brief Default destructor.
@@ -292,6 +292,7 @@ class GraphManager
292292
*/
293293
void update_node_entities_info();
294294

295+
eprosima::fastdds::dds::DomainId_t domain_id_;
295296
bool graph_changed_;
296297
bool display_on_change_;
297298
const char * enclave_;
@@ -319,4 +320,4 @@ class GraphManager
319320
} // namespace agent
320321
} // namespace uros
321322

322-
#endif // _UROS_AGENT_GRAPH_MANAGER_HPP
323+
#endif // _UROS_AGENT_GRAPH_MANAGER_HPP

‎micro_ros_agent/src/agent/Agent.cpp‎

Lines changed: 58 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,6 @@ namespace agent {
2222

2323
Agent::Agent()
2424
: xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance())
25-
, graph_manager_(nullptr)
2625
{
2726
}
2827

@@ -33,8 +32,6 @@ bool Agent::create(
3332
bool result = xrce_dds_agent_instance_.create(argc, argv);
3433
if (result)
3534
{
36-
graph_manager_.reset(new graph_manager::GraphManager());
37-
3835
/**
3936
* Add CREATE_PARTICIPANT callback.
4037
*/
@@ -43,6 +40,12 @@ bool Agent::create(
4340
([&](
4441
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
4542
{
43+
auto graph_manager_ =
44+
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
45+
participant->get_domain_id()
46+
)
47+
);
48+
4649
graph_manager_->add_participant(participant);
4750
});
4851
xrce_dds_agent_instance_.add_middleware_callback(
@@ -58,6 +61,12 @@ bool Agent::create(
5861
([&](
5962
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
6063
{
64+
auto graph_manager_ =
65+
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
66+
participant->get_domain_id()
67+
)
68+
);
69+
6170
graph_manager_->remove_participant(participant->guid());
6271
});
6372
xrce_dds_agent_instance_.add_middleware_callback(
@@ -75,6 +84,12 @@ bool Agent::create(
7584
const eprosima::fastdds::dds::DomainParticipant* participant,
7685
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
7786
{
87+
auto graph_manager_ =
88+
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
89+
participant->get_domain_id()
90+
)
91+
);
92+
7893
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
7994
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
8095
datawriter->get_instance_handle();
@@ -96,9 +111,16 @@ bool Agent::create(
96111
const eprosima::fastdds::dds::DomainParticipant *,
97112
const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
98113
([&](
99-
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
114+
const eprosima::fastdds::dds::DomainParticipant* participant,
100115
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
101116
{
117+
118+
auto graph_manager_ =
119+
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
120+
participant->get_domain_id()
121+
)
122+
);
123+
102124
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
103125
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
104126
datawriter->get_instance_handle();
@@ -122,6 +144,12 @@ bool Agent::create(
122144
const eprosima::fastdds::dds::DomainParticipant* participant,
123145
const eprosima::fastdds::dds::DataReader* datareader) -> void
124146
{
147+
auto graph_manager_ =
148+
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
149+
participant->get_domain_id()
150+
)
151+
);
152+
125153
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
126154
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
127155
datareader->get_instance_handle();
@@ -143,9 +171,15 @@ bool Agent::create(
143171
const eprosima::fastdds::dds::DomainParticipant *,
144172
const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
145173
([&](
146-
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
174+
const eprosima::fastdds::dds::DomainParticipant* participant,
147175
const eprosima::fastdds::dds::DataReader* datareader) -> void
148176
{
177+
auto graph_manager_ =
178+
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
179+
participant->get_domain_id()
180+
)
181+
);
182+
149183
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
150184
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
151185
datareader->get_instance_handle();
@@ -168,6 +202,25 @@ void Agent::run()
168202
return xrce_dds_agent_instance_.run();
169203
}
170204

205+
std::shared_ptr<graph_manager::GraphManager> Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id)
206+
{
207+
auto it = graph_manager_map_.find(domain_id);
208+
209+
if (it != graph_manager_map_.end()) {
210+
return it->second;
211+
}else{
212+
return graph_manager_map_.insert(
213+
std::pair<
214+
eprosima::fastdds::dds::DomainId_t,
215+
std::shared_ptr<graph_manager::GraphManager>
216+
>(
217+
domain_id,
218+
std::make_shared<graph_manager::GraphManager>(domain_id)
219+
)
220+
).first->second;
221+
}
222+
}
223+
171224
} // namespace agent
172225
} // namespace uros
173226
#endif // _UROS_AGENT_AGENT_CPP

‎micro_ros_agent/src/agent/graph_manager/graph_manager.cpp‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,10 @@ namespace uros {
2121
namespace agent {
2222
namespace graph_manager {
2323

24-
GraphManager::GraphManager()
24+
GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
2525
// : eprosima::fastrtps::ParticipantListener()
26-
: graph_changed_(false)
26+
: domain_id_(domain_id)
27+
, graph_changed_(false)
2728
, display_on_change_(false)
2829
, enclave_("/")
2930
, mtx_()
@@ -37,8 +38,6 @@ GraphManager::GraphManager()
3738
eprosima::fastdds::dds::TypeSupport>(new graph_manager::MicrorosGraphInfoTypeSupport()))
3839
{
3940
// Create DomainParticipant
40-
eprosima::fastdds::dds::DomainId_t domain_id(0);
41-
4241
eprosima::fastdds::dds::DomainParticipantQos participant_qos =
4342
eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos();
4443

@@ -54,7 +53,7 @@ GraphManager::GraphManager()
5453
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
5554

5655
participant_.reset(eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->
57-
create_participant(domain_id, participant_qos, participant_listener_.get()));
56+
create_participant(domain_id_, participant_qos, participant_listener_.get()));
5857

5958
// Register participant within typesupport
6059
participant_->register_type(*participant_info_typesupport_);

0 commit comments

Comments
 (0)