nicco/micro-ROS-Agent
1
0
Fork 0
mirror of https://github.com/micro-ROS/micro-ROS-Agent.git synced 2026-09-06 09:55:15 +02:00
Code Issues Packages Projects Releases Wiki Activity

Add services to graph manager (#127)

Browse Source 
Signed-off-by: Pablo Garrido <pablogs9@gmail.com>
This commit is contained in:
Pablo Garrido
2022-03-22 10:22:15 +01:00
committed by GitHub
parent ddefabfa8c
commit 6d67807737
1 changed files with 125 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
micro_ros_agent/src/agent/Agent.cpp
+125 -13
View File
@@ -79,14 +79,9 @@ bool Agent::create(
{ {
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter);
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( graph_manager_->associate_entity(
datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER); datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER);
}); });
xrce_dds_agent_instance_.add_middleware_callback( xrce_dds_agent_instance_.add_middleware_callback(
eprosima::uxr::Middleware::Kind::FASTDDS, eprosima::uxr::Middleware::Kind::FASTDDS,
@@ -106,12 +101,7 @@ bool Agent::create(
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed graph_manager_->remove_datawriter(datawriter->guid());
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( xrce_dds_agent_instance_.add_middleware_callback(
@@ -169,6 +159,128 @@ bool Agent::create(
eprosima::uxr::Middleware::Kind::FASTDDS, eprosima::uxr::Middleware::Kind::FASTDDS,
eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER, eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
std::move(on_delete_datareader)); std::move(on_delete_datareader));
/**
* Add CREATE_REQUESTER callback.
*/
std::function<void (
const eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataWriter *,
const eprosima::fastdds::dds::DataReader *)> on_create_requester
([&](
const eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataWriter* datawriter,
const eprosima::fastdds::dds::DataReader * datareader) -> void
{
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter);
graph_manager_->associate_entity(
datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER);
// TODO(pablogs): 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_REQUESTER,
std::move(on_create_requester));
/**
* Add DELETE_REQUESTER callback.
*/
std::function<void (
const eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataWriter *,
const eprosima::fastdds::dds::DataReader *)> on_delete_requester
([&](
const eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataWriter* datawriter,
const eprosima::fastdds::dds::DataReader * datareader) -> void
{
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
graph_manager_->remove_datawriter(datawriter->guid());
// TODO(pablogs): 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_REQUESTER,
std::move(on_delete_requester));
/**
* Add CREATE_REPLIER callback.
*/
std::function<void (
const eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataWriter *,
const eprosima::fastdds::dds::DataReader *)> on_create_replier
([&](
const eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataWriter* datawriter,
const eprosima::fastdds::dds::DataReader * datareader) -> void
{
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter);
graph_manager_->associate_entity(
datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER);
// TODO(pablogs): 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_REPLIER,
std::move(on_create_replier));
/**
* Add DELETE_REPLIER callback.
*/
std::function<void (
const eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataWriter *,
const eprosima::fastdds::dds::DataReader *)> on_delete_replier
([&](
const eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataWriter* datawriter,
const eprosima::fastdds::dds::DataReader * datareader) -> void
{
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
graph_manager_->remove_datawriter(datawriter->guid());
// TODO(pablogs): 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_REPLIER,
std::move(on_delete_replier));
} }
return result; return result;