Skip to content

Commit e9330bf

Browse files
jamoralpjamoralp
authored
Create graph manager after checking passed CLI arguments and launch xrce-dds server (#41)
1 parent 602976b commit e9330bf

File tree

1 file changed

+132
-125
lines changed

1 file changed

+132
-125
lines changed

‎micro_ros_agent/src/agent/Agent.cpp‎

Lines changed: 132 additions & 125 deletions
Original file line numberDiff line numberDiff line change
@@ -22,138 +22,145 @@ namespace agent {
2222

2323
Agent::Agent()
2424
: xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance())
25-
, graph_manager_(std::make_unique<graph_manager::GraphManager>())
25+
, graph_manager_(nullptr)
2626
{
27-
/**
28-
* Add CREATE_PARTICIPANT callback.
29-
*/
30-
std::function<void (
31-
const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
32-
([&](
33-
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
34-
{
35-
graph_manager_->add_participant(participant);
36-
});
37-
xrce_dds_agent_instance_.add_middleware_callback(
38-
eprosima::uxr::Middleware::Kind::FASTDDS,
39-
eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT,
40-
std::move(on_create_participant));
41-
42-
/**
43-
* Add REMOVE_PARTICIPANT callback.
44-
*/
45-
std::function<void (
46-
const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
47-
([&](
48-
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
49-
{
50-
graph_manager_->remove_participant(participant->guid());
51-
});
52-
xrce_dds_agent_instance_.add_middleware_callback(
53-
eprosima::uxr::Middleware::Kind::FASTDDS,
54-
eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT,
55-
std::move(on_delete_participant));
56-
57-
/**
58-
* Add CREATE_DATAWRITER callback.
59-
*/
60-
std::function<void (
61-
const eprosima::fastdds::dds::DomainParticipant *,
62-
const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
63-
([&](
64-
const eprosima::fastdds::dds::DomainParticipant* participant,
65-
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
66-
{
67-
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
68-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
69-
datawriter->get_instance_handle();
70-
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
71-
iHandle2GUID(instance_handle);
72-
graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
73-
graph_manager_->associate_entity(
74-
datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
75-
});
76-
xrce_dds_agent_instance_.add_middleware_callback(
77-
eprosima::uxr::Middleware::Kind::FASTDDS,
78-
eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER,
79-
std::move(on_create_datawriter));
80-
81-
/**
82-
* Add DELETE_DATAWRITER callback.
83-
*/
84-
std::function<void (
85-
const eprosima::fastdds::dds::DomainParticipant *,
86-
const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
87-
([&](
88-
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
89-
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
90-
{
91-
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
92-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
93-
datawriter->get_instance_handle();
94-
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
95-
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
96-
graph_manager_->remove_datawriter(datawriter_guid);
97-
});
98-
99-
xrce_dds_agent_instance_.add_middleware_callback(
100-
eprosima::uxr::Middleware::Kind::FASTDDS,
101-
eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER,
102-
std::move(on_delete_datawriter));
103-
104-
/**
105-
* Add CREATE_DATAREADER callback.
106-
*/
107-
std::function<void (
108-
const eprosima::fastdds::dds::DomainParticipant *,
109-
const eprosima::fastdds::dds::DataReader*)> on_create_datareader
110-
([&](
111-
const eprosima::fastdds::dds::DomainParticipant* participant,
112-
const eprosima::fastdds::dds::DataReader* datareader) -> void
113-
{
114-
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
115-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
116-
datareader->get_instance_handle();
117-
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
118-
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
119-
graph_manager_->add_datareader(datareader_guid, participant, datareader);
120-
graph_manager_->associate_entity(
121-
datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
122-
});
123-
xrce_dds_agent_instance_.add_middleware_callback(
124-
eprosima::uxr::Middleware::Kind::FASTDDS,
125-
eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER,
126-
std::move(on_create_datareader));
127-
128-
/**
129-
* Add DELETE_DATAREADER callback.
130-
*/
131-
std::function<void (
132-
const eprosima::fastdds::dds::DomainParticipant *,
133-
const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
134-
([&](
135-
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
136-
const eprosima::fastdds::dds::DataReader* datareader) -> void
137-
{
138-
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
139-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
140-
datareader->get_instance_handle();
141-
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
142-
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
143-
graph_manager_->remove_datareader(datareader_guid);
144-
});
145-
146-
xrce_dds_agent_instance_.add_middleware_callback(
147-
eprosima::uxr::Middleware::Kind::FASTDDS,
148-
eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
149-
std::move(on_delete_datareader));
15027
}
15128

15229
bool Agent::create(
15330
int argc,
15431
char** argv)
15532
{
156-
return xrce_dds_agent_instance_.create(argc, argv);
33+
bool result = xrce_dds_agent_instance_.create(argc, argv);
34+
if (result)
35+
{
36+
graph_manager_.reset(new graph_manager::GraphManager());
37+
38+
/**
39+
* Add CREATE_PARTICIPANT callback.
40+
*/
41+
std::function<void (
42+
const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
43+
([&](
44+
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
45+
{
46+
graph_manager_->add_participant(participant);
47+
});
48+
xrce_dds_agent_instance_.add_middleware_callback(
49+
eprosima::uxr::Middleware::Kind::FASTDDS,
50+
eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT,
51+
std::move(on_create_participant));
52+
53+
/**
54+
* Add REMOVE_PARTICIPANT callback.
55+
*/
56+
std::function<void (
57+
const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
58+
([&](
59+
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
60+
{
61+
graph_manager_->remove_participant(participant->guid());
62+
});
63+
xrce_dds_agent_instance_.add_middleware_callback(
64+
eprosima::uxr::Middleware::Kind::FASTDDS,
65+
eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT,
66+
std::move(on_delete_participant));
67+
68+
/**
69+
* Add CREATE_DATAWRITER callback.
70+
*/
71+
std::function<void (
72+
const eprosima::fastdds::dds::DomainParticipant *,
73+
const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
74+
([&](
75+
const eprosima::fastdds::dds::DomainParticipant* participant,
76+
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
77+
{
78+
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
79+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
80+
datawriter->get_instance_handle();
81+
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
82+
iHandle2GUID(instance_handle);
83+
graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
84+
graph_manager_->associate_entity(
85+
datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
86+
});
87+
xrce_dds_agent_instance_.add_middleware_callback(
88+
eprosima::uxr::Middleware::Kind::FASTDDS,
89+
eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER,
90+
std::move(on_create_datawriter));
91+
92+
/**
93+
* Add DELETE_DATAWRITER callback.
94+
*/
95+
std::function<void (
96+
const eprosima::fastdds::dds::DomainParticipant *,
97+
const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
98+
([&](
99+
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
100+
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
101+
{
102+
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
103+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
104+
datawriter->get_instance_handle();
105+
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
106+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
107+
graph_manager_->remove_datawriter(datawriter_guid);
108+
});
109+
110+
xrce_dds_agent_instance_.add_middleware_callback(
111+
eprosima::uxr::Middleware::Kind::FASTDDS,
112+
eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER,
113+
std::move(on_delete_datawriter));
114+
115+
/**
116+
* Add CREATE_DATAREADER callback.
117+
*/
118+
std::function<void (
119+
const eprosima::fastdds::dds::DomainParticipant *,
120+
const eprosima::fastdds::dds::DataReader*)> on_create_datareader
121+
([&](
122+
const eprosima::fastdds::dds::DomainParticipant* participant,
123+
const eprosima::fastdds::dds::DataReader* datareader) -> void
124+
{
125+
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
126+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
127+
datareader->get_instance_handle();
128+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
129+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
130+
graph_manager_->add_datareader(datareader_guid, participant, datareader);
131+
graph_manager_->associate_entity(
132+
datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
133+
});
134+
xrce_dds_agent_instance_.add_middleware_callback(
135+
eprosima::uxr::Middleware::Kind::FASTDDS,
136+
eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER,
137+
std::move(on_create_datareader));
138+
139+
/**
140+
* Add DELETE_DATAREADER callback.
141+
*/
142+
std::function<void (
143+
const eprosima::fastdds::dds::DomainParticipant *,
144+
const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
145+
([&](
146+
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
147+
const eprosima::fastdds::dds::DataReader* datareader) -> void
148+
{
149+
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
150+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
151+
datareader->get_instance_handle();
152+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
153+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
154+
graph_manager_->remove_datareader(datareader_guid);
155+
});
156+
157+
xrce_dds_agent_instance_.add_middleware_callback(
158+
eprosima::uxr::Middleware::Kind::FASTDDS,
159+
eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
160+
std::move(on_delete_datareader));
161+
}
162+
163+
return result;
157164
}
158165

159166
void Agent::run()

0 commit comments

Comments
 (0)