mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 09:22:18 +00:00
microRTPS: agent: match the code style from the PX4 Firmware
This commit is contained in:
TSC21
@@ -73,172 +73,182 @@ except AttributeError:
|
||||
#include "@(topic)_Subscriber.h"
|
||||
|
||||
@(topic)_Subscriber::@(topic)_Subscriber()
|
||||
: mp_participant(nullptr),
|
||||
mp_subscriber(nullptr)
|
||||
: mp_participant(nullptr),
|
||||
mp_subscriber(nullptr)
|
||||
{ }
|
||||
|
||||
@(topic)_Subscriber::~@(topic)_Subscriber()
|
||||
{
|
||||
Domain::removeParticipant(mp_participant);
|
||||
Domain::removeParticipant(mp_participant);
|
||||
}
|
||||
|
||||
bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue<uint8_t>* t_send_queue, const std::string& ns)
|
||||
bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv,
|
||||
std::mutex *t_send_queue_mutex, std::queue<uint8_t> *t_send_queue, const std::string &ns)
|
||||
{
|
||||
m_listener.topic_ID = topic_ID;
|
||||
m_listener.t_send_queue_cv = t_send_queue_cv;
|
||||
m_listener.t_send_queue_mutex = t_send_queue_mutex;
|
||||
m_listener.t_send_queue = t_send_queue;
|
||||
m_listener.topic_ID = topic_ID;
|
||||
m_listener.t_send_queue_cv = t_send_queue_cv;
|
||||
m_listener.t_send_queue_mutex = t_send_queue_mutex;
|
||||
m_listener.t_send_queue = t_send_queue;
|
||||
|
||||
// Create RTPSParticipant
|
||||
ParticipantAttributes PParam;
|
||||
// Create RTPSParticipant
|
||||
ParticipantAttributes PParam;
|
||||
@[if version.parse(fastrtps_version) < version.parse('2.0')]@
|
||||
PParam.rtps.builtin.domainId = 0;
|
||||
PParam.rtps.builtin.domainId = 0;
|
||||
@[else]@
|
||||
PParam.domainId = 0;
|
||||
PParam.domainId = 0;
|
||||
@[end if]@
|
||||
@[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@
|
||||
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
|
||||
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
|
||||
@[else]@
|
||||
PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
|
||||
PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
|
||||
@[end if]@
|
||||
std::string nodeName = ns;
|
||||
nodeName.append("@(topic)_subscriber");
|
||||
PParam.rtps.setName(nodeName.c_str());
|
||||
std::string nodeName = ns;
|
||||
nodeName.append("@(topic)_subscriber");
|
||||
PParam.rtps.setName(nodeName.c_str());
|
||||
|
||||
// Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use
|
||||
// only the localhost network for data sharing
|
||||
const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY");
|
||||
if (localhost_only && strcmp(localhost_only, "1") == 0) {
|
||||
// Create a custom network transport descriptor to whitelist the localhost
|
||||
auto localhostDescriptor = std::make_shared<UDPv4TransportDescriptor>();
|
||||
localhostDescriptor->interfaceWhiteList.emplace_back("127.0.0.1");
|
||||
// Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use
|
||||
// only the localhost network for data sharing
|
||||
const char *localhost_only = std::getenv("ROS_LOCALHOST_ONLY");
|
||||
|
||||
// Disable the built-in Transport Layer
|
||||
PParam.rtps.useBuiltinTransports = false;
|
||||
if (localhost_only && strcmp(localhost_only, "1") == 0) {
|
||||
// Create a custom network transport descriptor to whitelist the localhost
|
||||
auto localhostDescriptor = std::make_shared<UDPv4TransportDescriptor>();
|
||||
localhostDescriptor->interfaceWhiteList.emplace_back("127.0.0.1");
|
||||
|
||||
// Add the descriptor as a custom user transport
|
||||
PParam.rtps.userTransports.push_back(localhostDescriptor);
|
||||
}
|
||||
// Disable the built-in Transport Layer
|
||||
PParam.rtps.useBuiltinTransports = false;
|
||||
|
||||
mp_participant = Domain::createParticipant(PParam);
|
||||
if(mp_participant == nullptr)
|
||||
return false;
|
||||
// Add the descriptor as a custom user transport
|
||||
PParam.rtps.userTransports.push_back(localhostDescriptor);
|
||||
}
|
||||
|
||||
//Register the type
|
||||
Domain::registerType(mp_participant, static_cast<TopicDataType*>(&@(topic)DataType));
|
||||
mp_participant = Domain::createParticipant(PParam);
|
||||
|
||||
// Create Subscriber
|
||||
SubscriberAttributes Rparam;
|
||||
Rparam.topic.topicKind = NO_KEY;
|
||||
Rparam.topic.topicDataType = @(topic)DataType.getName();
|
||||
if (mp_participant == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Register the type
|
||||
Domain::registerType(mp_participant, static_cast<TopicDataType *>(&@(topic)DataType));
|
||||
|
||||
// Create Subscriber
|
||||
SubscriberAttributes Rparam;
|
||||
Rparam.topic.topicKind = NO_KEY;
|
||||
Rparam.topic.topicDataType = @(topic)DataType.getName();
|
||||
@[if ros2_distro]@
|
||||
@[ if ros2_distro == "ardent"]@
|
||||
Rparam.qos.m_partition.push_back("rt");
|
||||
std::string topicName = ns;
|
||||
topicName.append("@(topic)_PubSubTopic");
|
||||
Rparam.topic.topicName = topicName;
|
||||
Rparam.qos.m_partition.push_back("rt");
|
||||
std::string topicName = ns;
|
||||
topicName.append("@(topic)_PubSubTopic");
|
||||
Rparam.topic.topicName = topicName;
|
||||
@[ else]@
|
||||
std::string topicName = "rt/";
|
||||
topicName.append(ns);
|
||||
topicName.append("@(topic)_PubSubTopic");
|
||||
Rparam.topic.topicName = topicName;
|
||||
std::string topicName = "rt/";
|
||||
topicName.append(ns);
|
||||
topicName.append("@(topic)_PubSubTopic");
|
||||
Rparam.topic.topicName = topicName;
|
||||
@[ end if]@
|
||||
@[else]@
|
||||
std::string topicName = ns;
|
||||
topicName.append("@(topic)PubSubTopic");
|
||||
Rparam.topic.topicName = topicName;
|
||||
std::string topicName = ns;
|
||||
topicName.append("@(topic)PubSubTopic");
|
||||
Rparam.topic.topicName = topicName;
|
||||
@[end if]@
|
||||
mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast<SubscriberListener*>(&m_listener));
|
||||
if(mp_subscriber == nullptr)
|
||||
return false;
|
||||
return true;
|
||||
mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast<SubscriberListener *>(&m_listener));
|
||||
|
||||
if (mp_subscriber == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, MatchingInfo& info)
|
||||
void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber *sub, MatchingInfo &info)
|
||||
{
|
||||
@# Since the time sync runs on the bridge itself, it is required that there is a
|
||||
@# match between two topics of the same entity
|
||||
@[if topic != 'Timesync' and topic != 'timesync']@
|
||||
// The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain
|
||||
// are the same for all its subcomponents (publishers, subscribers)
|
||||
bool is_different_endpoint = false;
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
if (sub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) {
|
||||
is_different_endpoint = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
// The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain
|
||||
// are the same for all its subcomponents (publishers, subscribers)
|
||||
bool is_different_endpoint = false;
|
||||
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
if (sub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) {
|
||||
is_different_endpoint = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// If the matching happens for the same entity, do not make a match
|
||||
if (is_different_endpoint) {
|
||||
if (info.status == MATCHED_MATCHING) {
|
||||
n_matched++;
|
||||
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl;
|
||||
|
||||
} else {
|
||||
n_matched--;
|
||||
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// If the matching happens for the same entity, do not make a match
|
||||
if (is_different_endpoint) {
|
||||
if (info.status == MATCHED_MATCHING) {
|
||||
n_matched++;
|
||||
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl;
|
||||
} else {
|
||||
n_matched--;
|
||||
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl;
|
||||
}
|
||||
}
|
||||
@[else]@
|
||||
(void)sub;
|
||||
(void)sub;
|
||||
|
||||
if (info.status == MATCHED_MATCHING) {
|
||||
n_matched++;
|
||||
} else {
|
||||
n_matched--;
|
||||
}
|
||||
if (info.status == MATCHED_MATCHING) {
|
||||
n_matched++;
|
||||
|
||||
} else {
|
||||
n_matched--;
|
||||
}
|
||||
@[end if]@
|
||||
}
|
||||
|
||||
void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber* sub)
|
||||
void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber *sub)
|
||||
{
|
||||
if (n_matched > 0) {
|
||||
std::unique_lock<std::mutex> has_msg_lock(has_msg_mutex);
|
||||
if(has_msg.load() == true) // Check if msg has been fetched
|
||||
{
|
||||
has_msg_cv.wait(has_msg_lock); // Wait till msg has been fetched
|
||||
}
|
||||
has_msg_lock.unlock();
|
||||
if (n_matched > 0) {
|
||||
std::unique_lock<std::mutex> has_msg_lock(has_msg_mutex);
|
||||
|
||||
// Take data
|
||||
if(sub->takeNextData(&msg, &m_info))
|
||||
{
|
||||
if(m_info.sampleKind == ALIVE)
|
||||
{
|
||||
std::unique_lock<std::mutex> lk(*t_send_queue_mutex);
|
||||
if (has_msg.load() == true) { // Check if msg has been fetched
|
||||
has_msg_cv.wait(has_msg_lock); // Wait till msg has been fetched
|
||||
}
|
||||
|
||||
++n_msg;
|
||||
has_msg = true;
|
||||
has_msg_lock.unlock();
|
||||
|
||||
t_send_queue->push(topic_ID);
|
||||
lk.unlock();
|
||||
t_send_queue_cv->notify_one();
|
||||
// Take data
|
||||
if (sub->takeNextData(&msg, &m_info)) {
|
||||
if (m_info.sampleKind == ALIVE) {
|
||||
std::unique_lock<std::mutex> lk(*t_send_queue_mutex);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
++n_msg;
|
||||
has_msg = true;
|
||||
|
||||
t_send_queue->push(topic_ID);
|
||||
lk.unlock();
|
||||
t_send_queue_cv->notify_one();
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool @(topic)_Subscriber::hasMsg()
|
||||
{
|
||||
if (m_listener.n_matched > 0) {
|
||||
return m_listener.has_msg.load();
|
||||
}
|
||||
if (m_listener.n_matched > 0) {
|
||||
return m_listener.has_msg.load();
|
||||
}
|
||||
|
||||
return false;
|
||||
return false;
|
||||
}
|
||||
|
||||
@(topic)_msg_t @(topic)_Subscriber::getMsg()
|
||||
{
|
||||
return m_listener.msg;
|
||||
return m_listener.msg;
|
||||
}
|
||||
|
||||
void @(topic)_Subscriber::unlockMsg()
|
||||
{
|
||||
if (m_listener.n_matched > 0) {
|
||||
std::unique_lock<std::mutex> has_msg_lock(m_listener.has_msg_mutex);
|
||||
m_listener.has_msg = false;
|
||||
has_msg_lock.unlock();
|
||||
m_listener.has_msg_cv.notify_one();
|
||||
}
|
||||
if (m_listener.n_matched > 0) {
|
||||
std::unique_lock<std::mutex> has_msg_lock(m_listener.has_msg_mutex);
|
||||
m_listener.has_msg = false;
|
||||
has_msg_lock.unlock();
|
||||
m_listener.has_msg_cv.notify_one();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user