1. 增加逻辑处理程序。
baocm
8 months ago
parent
3dbf090a91
commit
47336c9a07
@ -0,0 +1,518 @@
|
||||
// Copyright 2016 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.
|
||||
|
||||
/*!
|
||||
* @file Publisher.cxx
|
||||
* This file contains the implementation of the publisher functions.
|
||||
*
|
||||
* This file was generated by the tool fastddsgen.
|
||||
*/
|
||||
|
||||
#include "Publisher.hpp"
|
||||
|
||||
#include <condition_variable>
|
||||
#include <csignal>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
|
||||
#include <fastdds/dds/log/Log.hpp>
|
||||
#include <fastdds/dds/publisher/DataWriter.hpp>
|
||||
#include <fastdds/dds/publisher/Publisher.hpp>
|
||||
#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
|
||||
#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
|
||||
|
||||
#include "SystemPubSubTypes.hpp"
|
||||
#include "json.hpp"
|
||||
#include "msg.hpp"
|
||||
|
||||
using namespace eprosima::fastdds::dds;
|
||||
using json = nlohmann::json;
|
||||
|
||||
PublisherApp::PublisherApp(
|
||||
const int& domain_id)
|
||||
: factory_(nullptr)
|
||||
, participant_(nullptr)
|
||||
, publisher_(nullptr)
|
||||
, bar_topic_(nullptr)
|
||||
, bar_writer_(nullptr)
|
||||
, bar_type_(new WeighingSystem::BarCommandUpdatePubSubType())
|
||||
, light_topic_(nullptr)
|
||||
, light_writer_(nullptr)
|
||||
, light_type_(new WeighingSystem::LightsCommandUpdatePubSubType())
|
||||
, summary_topic_(nullptr)
|
||||
, summary_writer_(nullptr)
|
||||
, summary_type_(new WeighingSystem::SummaryUpdatePubSubType())
|
||||
, matched_(0)
|
||||
, samples_sent_(0)
|
||||
, stop_(false)
|
||||
{
|
||||
//
|
||||
|
||||
// Create the participant
|
||||
DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT;
|
||||
pqos.name("Core_pub_participant");
|
||||
pqos.wire_protocol().builtin.discovery_config.leaseDuration = Duration_t(60, 0);
|
||||
pqos.wire_protocol().builtin.discovery_config.leaseDuration_announcementperiod = Duration_t(30, 0);
|
||||
factory_ = DomainParticipantFactory::get_shared_instance();
|
||||
participant_ = factory_->create_participant(domain_id, pqos, nullptr, StatusMask::none());
|
||||
if (participant_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("Core Participant initialization failed");
|
||||
}
|
||||
|
||||
// Register the type
|
||||
bar_type_.register_type(participant_);
|
||||
light_type_.register_type(participant_);
|
||||
summary_type_.register_type(participant_);
|
||||
|
||||
// Create the publisher
|
||||
PublisherQos pub_qos = PUBLISHER_QOS_DEFAULT;
|
||||
participant_->get_default_publisher_qos(pub_qos);
|
||||
publisher_ = participant_->create_publisher(pub_qos, nullptr, StatusMask::none());
|
||||
if (publisher_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("Core Publisher initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
TopicQos topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
bar_topic_ = participant_->create_topic("BarCommandUpdate", bar_type_.get_type_name(), topic_qos);
|
||||
if (bar_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("BarCommandUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the data writer
|
||||
DataWriterQos writer_qos = DATAWRITER_QOS_DEFAULT;
|
||||
publisher_->get_default_datawriter_qos(writer_qos);
|
||||
writer_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
writer_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
writer_qos.durability().kind = DurabilityQosPolicyKind::VOLATILE_DURABILITY_QOS;
|
||||
writer_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
writer_qos.history().depth = 1;
|
||||
writer_qos.resource_limits().max_samples = 200;
|
||||
writer_qos.resource_limits().max_instances = 1;
|
||||
writer_qos.resource_limits().max_samples_per_instance = 100;
|
||||
writer_qos.data_sharing().off();
|
||||
bar_writer_ = publisher_->create_datawriter(bar_topic_, writer_qos, this, StatusMask::all());
|
||||
if (bar_writer_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::BarUpdate DataWriter initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
light_topic_ = participant_->create_topic("LightsCommandUpdate", light_type_.get_type_name(), topic_qos);
|
||||
if (light_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("LightsCommandUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the data writer
|
||||
writer_qos = DATAWRITER_QOS_DEFAULT;
|
||||
publisher_->get_default_datawriter_qos(writer_qos);
|
||||
writer_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
writer_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
writer_qos.durability().kind = DurabilityQosPolicyKind::VOLATILE_DURABILITY_QOS;
|
||||
writer_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
writer_qos.history().depth = 1;
|
||||
writer_qos.resource_limits().max_samples = 200;
|
||||
writer_qos.resource_limits().max_instances = 1;
|
||||
writer_qos.resource_limits().max_samples_per_instance = 100;
|
||||
writer_qos.data_sharing().off();
|
||||
light_writer_ = publisher_->create_datawriter(light_topic_, writer_qos, this, StatusMask::all());
|
||||
if (light_writer_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::LightsUpdate DataWriter initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
summary_topic_ = participant_->create_topic("SummaryUpdate", summary_type_.get_type_name(), topic_qos);
|
||||
if (summary_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("SummaryUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the data writer
|
||||
writer_qos = DATAWRITER_QOS_DEFAULT;
|
||||
publisher_->get_default_datawriter_qos(writer_qos);
|
||||
writer_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
writer_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
writer_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
|
||||
writer_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
writer_qos.history().depth = 1;
|
||||
writer_qos.resource_limits().max_samples = 200;
|
||||
writer_qos.resource_limits().max_instances = 1;
|
||||
writer_qos.resource_limits().max_samples_per_instance = 100;
|
||||
writer_qos.data_sharing().off();
|
||||
summary_writer_ = publisher_->create_datawriter(summary_topic_, writer_qos, this, StatusMask::all());
|
||||
if (summary_writer_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::SummaryUpdate DataWriter initialization failed");
|
||||
}
|
||||
}
|
||||
|
||||
PublisherApp::~PublisherApp()
|
||||
{
|
||||
if (nullptr != participant_)
|
||||
{
|
||||
// Delete DDS entities contained within the DomainParticipant
|
||||
participant_->delete_contained_entities();
|
||||
|
||||
// Delete DomainParticipant
|
||||
factory_->delete_participant(participant_);
|
||||
}
|
||||
}
|
||||
|
||||
void PublisherApp::on_publication_matched(
|
||||
DataWriter* writer,
|
||||
const PublicationMatchedStatus& info)
|
||||
{
|
||||
if (info.current_count_change == 1)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
matched_ = info.current_count;
|
||||
}
|
||||
std::cout << writer->get_topic()->get_name() << " Publisher matched." << std::endl;
|
||||
cv_.notify_one();
|
||||
}
|
||||
else if (info.current_count_change == -1)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
matched_ = info.current_count;
|
||||
}
|
||||
std::cout << writer->get_topic()->get_name() << " Publisher unmatched." << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << info.current_count_change
|
||||
<< " is not a valid value for PublicationMatchedStatus current count change" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
bool PublisherApp::bar_ctrl(const std::map<std::string, std::string>& ctrl)
|
||||
{
|
||||
WeighingSystem::BarCommandUpdate cmd;
|
||||
|
||||
for (const auto &[port, opt] : ctrl)
|
||||
{
|
||||
if (port == "FrontBar")
|
||||
{
|
||||
if (opt == "up")
|
||||
{
|
||||
cmd.FrontBarSignalUp() = 1;
|
||||
cmd.FrontBarSignalDown() = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd.FrontBarSignalUp() = 0;
|
||||
cmd.FrontBarSignalDown() = 1;
|
||||
}
|
||||
cmd.FrontBarEnable() = 1;
|
||||
}
|
||||
else if (port == "BackBar")
|
||||
{
|
||||
if (opt == "up")
|
||||
{
|
||||
cmd.BackBarSignalUp() = 1;
|
||||
cmd.BackBarSignalDown() = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd.BackBarSignalUp() = 0;
|
||||
cmd.BackBarSignalDown() = 1;
|
||||
}
|
||||
cmd.BackBarEnable() = 1;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "send bar ctrl cmd" << std::endl;
|
||||
bar_writer_->write(&cmd);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PublisherApp::light_ctrl(const std::map<std::string, std::string>& ctrl)
|
||||
{
|
||||
WeighingSystem::LightsCommandUpdate cmd;
|
||||
|
||||
for (const auto &[port, opt] : ctrl)
|
||||
{
|
||||
if (port == "FrontLED")
|
||||
{
|
||||
if (opt == "red")
|
||||
{
|
||||
cmd.FrontLEDSignal() = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd.FrontLEDSignal() = 2;
|
||||
}
|
||||
cmd.FrontLEDEnable() = 1;
|
||||
}
|
||||
else if (port == "BackLED")
|
||||
{
|
||||
if (opt == "red")
|
||||
{
|
||||
cmd.BackLEDSignal() = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd.BackLEDSignal() = 2;
|
||||
}
|
||||
cmd.BackLEDEnable() = 1;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "send light ctrl cmd" << std::endl;
|
||||
light_writer_->write(&cmd);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PublisherApp::run(std::shared_ptr<mqtt::async_client> dev)
|
||||
{
|
||||
uint8_t send_sum = 0;
|
||||
std::string license_no = "";
|
||||
float stable_weight = 0;
|
||||
|
||||
while (!is_stopped())
|
||||
{
|
||||
//dds msg
|
||||
std::unique_lock<std::mutex> dds_lock(DdsMsgData::queue_cv_mtx_, std::try_to_lock);
|
||||
if (dds_lock.owns_lock())
|
||||
{
|
||||
if (!DdsMsgData::LicenseSnapUpdate_queue_.empty())
|
||||
{
|
||||
WeighingSystem::LicenseSnapUpdate info = std::move(DdsMsgData::LicenseSnapUpdate_queue_.front());
|
||||
DdsMsgData::LicenseSnapUpdate_queue_.pop();
|
||||
dds_lock.unlock();
|
||||
|
||||
std::cout << "车牌: " << info.License() << std::endl;
|
||||
std::cout << "类型: " << info.NewTag() << std::endl;
|
||||
|
||||
if (license_no == "")
|
||||
{
|
||||
license_no = info.NewTag();
|
||||
}
|
||||
}
|
||||
else if (!DdsMsgData::ScaleInfo_queue_.empty())
|
||||
{
|
||||
WeighingSystem::ScaleInfo info = std::move(DdsMsgData::ScaleInfo_queue_.front());
|
||||
DdsMsgData::ScaleInfo_queue_.pop();
|
||||
dds_lock.unlock();
|
||||
|
||||
std::cout << "有车: " << info.HasVehicle() << std::endl;
|
||||
std::cout << "稳定: " << info.WeightOK() << std::endl;
|
||||
std::cout << "设备状态: " << info.State() << std::endl;
|
||||
std::cout << "实时重量: " << info.Value() << std::endl;
|
||||
std::cout << "稳定重量: " << info.StableValue() << std::endl;
|
||||
|
||||
if (info.WeightOK() == 1)
|
||||
{
|
||||
stable_weight = info.StableValue();
|
||||
}
|
||||
else
|
||||
{
|
||||
stable_weight = 0;
|
||||
send_sum = 0;
|
||||
}
|
||||
|
||||
if (info.HasVehicle() == 0)
|
||||
{
|
||||
license_no = "";
|
||||
}
|
||||
}
|
||||
else if (!DdsMsgData::BarUpdate_queue_.empty())
|
||||
{
|
||||
WeighingSystem::BarUpdate info = std::move(DdsMsgData::BarUpdate_queue_.front());
|
||||
DdsMsgData::BarUpdate_queue_.pop();
|
||||
dds_lock.unlock();
|
||||
|
||||
std::cout << "前拦车器状态: " << info.FrontBarState() << std::endl;
|
||||
std::cout << "后拦车器状态: " << info.BackBarState() << std::endl;
|
||||
|
||||
if (dev != nullptr)
|
||||
{
|
||||
if (dev->is_connected())
|
||||
{
|
||||
json j;
|
||||
j["bar_state"]["FrontBar"] = info.FrontBarState();
|
||||
j["bar_state"]["BackBar"] = info.BackBarState();
|
||||
|
||||
auto mqtt_msg = mqtt::make_message("response", j.dump());
|
||||
mqtt_msg->set_qos(0);
|
||||
mqtt_msg->set_retained(false);
|
||||
dev->publish(mqtt_msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!DdsMsgData::LightsUpdate_queue_.empty())
|
||||
{
|
||||
WeighingSystem::LightsUpdate info = std::move(DdsMsgData::LightsUpdate_queue_.front());
|
||||
DdsMsgData::LightsUpdate_queue_.pop();
|
||||
dds_lock.unlock();
|
||||
|
||||
std::cout << "前红绿灯状态: " << info.FrontLEDState() << std::endl;
|
||||
std::cout << "后红绿灯状态: " << info.BackLEDState() << std::endl;
|
||||
|
||||
if (dev != nullptr)
|
||||
{
|
||||
if (dev->is_connected())
|
||||
{
|
||||
json j;
|
||||
j["light_state"]["FrontLED"] = info.FrontLEDState();
|
||||
j["light_state"]["BackLED"] = info.FrontLEDState();
|
||||
|
||||
auto mqtt_msg = mqtt::make_message("response", j.dump());
|
||||
mqtt_msg->set_qos(0);
|
||||
mqtt_msg->set_retained(false);
|
||||
dev->publish(mqtt_msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!DdsMsgData::InfraredUpdate_queue_.empty())
|
||||
{
|
||||
WeighingSystem::InfraredUpdate info = std::move(DdsMsgData::InfraredUpdate_queue_.front());
|
||||
DdsMsgData::InfraredUpdate_queue_.pop();
|
||||
dds_lock.unlock();
|
||||
|
||||
std::cout << "前红外对射状态: " << info.FrontResistanceSignal() << std::endl;
|
||||
std::cout << "后红外对射状态: " << info.BackResistanceSignal() << std::endl;
|
||||
|
||||
if (dev != nullptr)
|
||||
{
|
||||
if (dev->is_connected())
|
||||
{
|
||||
json j;
|
||||
j["infrared_state"]["FrontResistance"] = info.FrontResistanceSignal();
|
||||
j["infrared_state"]["BackResistance"] = info.BackResistanceSignal();
|
||||
|
||||
auto mqtt_msg = mqtt::make_message("response", j.dump());
|
||||
mqtt_msg->set_qos(0);
|
||||
mqtt_msg->set_retained(false);
|
||||
dev->publish(mqtt_msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!DdsMsgData::WeightInfoOk_queue_.empty())
|
||||
{
|
||||
WeighingSystem::WeightInfoOk info = std::move(DdsMsgData::WeightInfoOk_queue_.front());
|
||||
DdsMsgData::WeightInfoOk_queue_.pop();
|
||||
dds_lock.unlock();
|
||||
}
|
||||
else if (!DdsMsgData::WeightInfoError_queue_.empty())
|
||||
{
|
||||
WeighingSystem::WeightInfoError info = std::move(DdsMsgData::WeightInfoError_queue_.front());
|
||||
DdsMsgData::WeightInfoError_queue_.pop();
|
||||
dds_lock.unlock();
|
||||
|
||||
send_sum = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
dds_lock.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
// mqtt msg
|
||||
std::unique_lock<std::mutex> mqtt_lock(MqttMsgData::queue_cv_mtx_, std::try_to_lock);
|
||||
if (mqtt_lock.owns_lock())
|
||||
{
|
||||
if (!MqttMsgData::Mqtt_msg_queue_.empty())
|
||||
{
|
||||
mqtt::const_message_ptr msg = std::move(MqttMsgData::Mqtt_msg_queue_.front());
|
||||
MqttMsgData::Mqtt_msg_queue_.pop();
|
||||
mqtt_lock.unlock();
|
||||
|
||||
json cmd = json::parse(msg->to_string());
|
||||
|
||||
if (cmd.contains("bar_ctrl"))
|
||||
{
|
||||
std::map<std::string, std::string> ctrl = cmd["bar_ctrl"].get<std::map<std::string, std::string>>();
|
||||
bar_ctrl(ctrl);
|
||||
}
|
||||
else if (cmd.contains("light_ctrl"))
|
||||
{
|
||||
std::map<std::string, std::string> ctrl = cmd["light_ctrl"].get<std::map<std::string, std::string>>();
|
||||
light_ctrl(ctrl);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
mqtt_lock.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
// logic
|
||||
if (send_sum == 0)
|
||||
{
|
||||
if ((stable_weight != 0) && (license_no != ""))
|
||||
{
|
||||
std::cout << "send summary" << std::endl;
|
||||
WeighingSystem::SummaryUpdate info;
|
||||
info.License() = license_no;
|
||||
info.StableValue() = stable_weight;
|
||||
summary_writer_->write(&info);
|
||||
send_sum = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Wait for period or stop event
|
||||
std::unique_lock<std::mutex> period_lock(mutex_);
|
||||
cv_.wait_for(period_lock, std::chrono::milliseconds(period_ms_), [this]()
|
||||
{
|
||||
return is_stopped();
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool PublisherApp::publish()
|
||||
{
|
||||
bool ret = false;
|
||||
// Wait for the data endpoints discovery
|
||||
std::unique_lock<std::mutex> matched_lock(mutex_);
|
||||
cv_.wait(matched_lock, [&]()
|
||||
{
|
||||
// at least one has been discovered
|
||||
return ((matched_ > 0) || is_stopped());
|
||||
});
|
||||
|
||||
if (!is_stopped())
|
||||
{
|
||||
/* Initialize your structure here */
|
||||
WeighingSystem::BarCommandUpdate sample_;
|
||||
ret = (RETCODE_OK == bar_writer_->write(&sample_));
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool PublisherApp::is_stopped()
|
||||
{
|
||||
return stop_.load();
|
||||
}
|
||||
|
||||
void PublisherApp::stop()
|
||||
{
|
||||
stop_.store(true);
|
||||
cv_.notify_one();
|
||||
}
|
||||
@ -0,0 +1,85 @@
|
||||
// Copyright 2016 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.
|
||||
|
||||
/*!
|
||||
* @file PublisherApp.hpp
|
||||
* This header file contains the declaration of the publisher functions.
|
||||
*
|
||||
* This file was generated by the tool fastddsgen.
|
||||
*/
|
||||
|
||||
#ifndef FAST_DDS_GENERATED__PUBLISHERAPP_HPP
|
||||
#define FAST_DDS_GENERATED__PUBLISHERAPP_HPP
|
||||
|
||||
#include <condition_variable>
|
||||
|
||||
#include <fastdds/dds/domain/DomainParticipant.hpp>
|
||||
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
|
||||
#include <fastdds/dds/publisher/DataWriterListener.hpp>
|
||||
#include <fastdds/dds/topic/TypeSupport.hpp>
|
||||
|
||||
#include "mqtt/async_client.h"
|
||||
|
||||
class PublisherApp : public eprosima::fastdds::dds::DataWriterListener
|
||||
{
|
||||
public:
|
||||
|
||||
PublisherApp(
|
||||
const int& domain_id);
|
||||
|
||||
~PublisherApp();
|
||||
|
||||
//! Publisher matched method
|
||||
void on_publication_matched(
|
||||
eprosima::fastdds::dds::DataWriter* writer,
|
||||
const eprosima::fastdds::dds::PublicationMatchedStatus& info) override;
|
||||
|
||||
//! Run publisher
|
||||
void run(std::shared_ptr<mqtt::async_client> dev);
|
||||
|
||||
//! Trigger the end of execution
|
||||
void stop();
|
||||
|
||||
bool bar_ctrl(const std::map<std::string, std::string>& ctrl);
|
||||
bool light_ctrl(const std::map<std::string, std::string>& ctrl);
|
||||
|
||||
private:
|
||||
|
||||
//! Return the current state of execution
|
||||
bool is_stopped();
|
||||
|
||||
//! Publish a sample
|
||||
bool publish();
|
||||
|
||||
std::shared_ptr<eprosima::fastdds::dds::DomainParticipantFactory> factory_;
|
||||
eprosima::fastdds::dds::DomainParticipant* participant_;
|
||||
eprosima::fastdds::dds::Publisher* publisher_;
|
||||
eprosima::fastdds::dds::Topic* bar_topic_;
|
||||
eprosima::fastdds::dds::DataWriter* bar_writer_;
|
||||
eprosima::fastdds::dds::TypeSupport bar_type_;
|
||||
eprosima::fastdds::dds::Topic* light_topic_;
|
||||
eprosima::fastdds::dds::DataWriter* light_writer_;
|
||||
eprosima::fastdds::dds::TypeSupport light_type_;
|
||||
eprosima::fastdds::dds::Topic* summary_topic_;
|
||||
eprosima::fastdds::dds::DataWriter* summary_writer_;
|
||||
eprosima::fastdds::dds::TypeSupport summary_type_;
|
||||
std::condition_variable cv_;
|
||||
int32_t matched_;
|
||||
std::mutex mutex_;
|
||||
const uint32_t period_ms_ = 100; // in ms
|
||||
uint16_t samples_sent_;
|
||||
std::atomic<bool> stop_;
|
||||
};
|
||||
|
||||
#endif // FAST_DDS_GENERATED__PUBLISHERAPP_HPP
|
||||
@ -0,0 +1,501 @@
|
||||
// Copyright 2016 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.
|
||||
|
||||
/*!
|
||||
* @file Subscriber.cxx
|
||||
* This file contains the implementation of the subscriber functions.
|
||||
*
|
||||
* This file was generated by the tool fastddsgen.
|
||||
*/
|
||||
|
||||
#include "Subscriber.hpp"
|
||||
|
||||
#include <condition_variable>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <fastdds/dds/core/status/SubscriptionMatchedStatus.hpp>
|
||||
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
|
||||
#include <fastdds/dds/subscriber/DataReader.hpp>
|
||||
#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>
|
||||
#include <fastdds/dds/subscriber/qos/SubscriberQos.hpp>
|
||||
#include <fastdds/dds/subscriber/SampleInfo.hpp>
|
||||
#include <fastdds/dds/subscriber/Subscriber.hpp>
|
||||
|
||||
#include "SystemPubSubTypes.hpp"
|
||||
|
||||
#include "msg.hpp"
|
||||
|
||||
using namespace eprosima::fastdds::dds;
|
||||
|
||||
SubscriberApp::SubscriberApp(
|
||||
const int& domain_id)
|
||||
: factory_(nullptr)
|
||||
, participant_(nullptr)
|
||||
, subscriber_(nullptr)
|
||||
, bar_topic_(nullptr)
|
||||
, bar_reader_(nullptr)
|
||||
, bar_type_(new WeighingSystem::BarUpdatePubSubType())
|
||||
, light_topic_(nullptr)
|
||||
, light_reader_(nullptr)
|
||||
, light_type_(new WeighingSystem::LightsUpdatePubSubType())
|
||||
, infraredinfo_topic_(nullptr)
|
||||
, infraredinfo_reader_(nullptr)
|
||||
, infraredinfo_type_(new WeighingSystem::InfraredUpdatePubSubType())
|
||||
, infraredcommand_topic_(nullptr)
|
||||
, infraredcommand_reader_(nullptr)
|
||||
, infraredcommand_type_(new WeighingSystem::InfraredCommandUpdatePubSubType())
|
||||
, license_topic_(nullptr)
|
||||
, license_reader_(nullptr)
|
||||
, license_type_(new WeighingSystem::LicenseSnapUpdatePubSubType())
|
||||
, scaleinfo_topic_(nullptr)
|
||||
, scaleinfo_reader_(nullptr)
|
||||
, scaleinfo_type_(new WeighingSystem::ScaleInfoPubSubType())
|
||||
, weightinfook_topic_(nullptr)
|
||||
, weightinfook_reader_(nullptr)
|
||||
, weightinfook_type_(new WeighingSystem::WeightInfoOkPubSubType())
|
||||
, weightinfoerror_topic_(nullptr)
|
||||
, weightinfoerror_reader_(nullptr)
|
||||
, weightinfoerror_type_(new WeighingSystem::WeightInfoErrorPubSubType())
|
||||
, samples_received_(0)
|
||||
, stop_(false)
|
||||
{
|
||||
// Create the participant
|
||||
DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT;
|
||||
pqos.name("WeighingSystem_sub_participant");
|
||||
pqos.wire_protocol().builtin.discovery_config.leaseDuration = Duration_t(60, 0);
|
||||
pqos.wire_protocol().builtin.discovery_config.leaseDuration_announcementperiod = Duration_t(30, 0);
|
||||
factory_ = DomainParticipantFactory::get_shared_instance();
|
||||
participant_ = factory_->create_participant(domain_id, pqos, nullptr, StatusMask::none());
|
||||
if (participant_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem Participant initialization failed");
|
||||
}
|
||||
|
||||
// Register the type
|
||||
bar_type_.register_type(participant_);
|
||||
light_type_.register_type(participant_);
|
||||
infraredinfo_type_.register_type(participant_);
|
||||
infraredcommand_type_.register_type(participant_);
|
||||
license_type_.register_type(participant_);
|
||||
scaleinfo_type_.register_type(participant_);
|
||||
weightinfook_type_.register_type(participant_);
|
||||
weightinfoerror_type_.register_type(participant_);
|
||||
|
||||
// Create the subscriber
|
||||
SubscriberQos sub_qos = SUBSCRIBER_QOS_DEFAULT;
|
||||
participant_->get_default_subscriber_qos(sub_qos);
|
||||
subscriber_ = participant_->create_subscriber(sub_qos, nullptr, StatusMask::none());
|
||||
if (subscriber_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem Subscriber initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
TopicQos topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
bar_topic_ = participant_->create_topic("BarUpdate", bar_type_.get_type_name(), topic_qos);
|
||||
if (bar_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::BarUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
DataReaderQos reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
bar_reader_ = subscriber_->create_datareader(bar_topic_, reader_qos, this, StatusMask::all());
|
||||
if (bar_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::BarUpdate DataReader initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
light_topic_ = participant_->create_topic("LightsUpdate", light_type_.get_type_name(), topic_qos);
|
||||
if (light_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::LightsUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
light_reader_ = subscriber_->create_datareader(light_topic_, reader_qos, this, StatusMask::all());
|
||||
if (light_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::LightsUpdate DataReader initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
infraredinfo_topic_ = participant_->create_topic("InfraredUpdate", infraredinfo_type_.get_type_name(), topic_qos);
|
||||
if (infraredinfo_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::InfraredUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
infraredinfo_reader_ = subscriber_->create_datareader(infraredinfo_topic_, reader_qos, this, StatusMask::all());
|
||||
if (infraredinfo_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::InfraredUpdate DataReader initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
infraredcommand_topic_ = participant_->create_topic("InfraredCommandUpdate", infraredcommand_type_.get_type_name(), topic_qos);
|
||||
if (infraredcommand_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::InfraredCommandUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
infraredcommand_reader_ = subscriber_->create_datareader(infraredcommand_topic_, reader_qos, this, StatusMask::all());
|
||||
if (infraredcommand_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::InfraredCommandUpdate DataReader initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
license_topic_ = participant_->create_topic("LicenseSnapUpdate", license_type_.get_type_name(), topic_qos);
|
||||
if (license_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::LicenseSnapUpdate Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::VOLATILE_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
license_reader_ = subscriber_->create_datareader(license_topic_, reader_qos, this, StatusMask::all());
|
||||
if (license_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::LicenseSnapUpdate DataReader initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
scaleinfo_topic_ = participant_->create_topic("ScaleInfo", scaleinfo_type_.get_type_name(), topic_qos);
|
||||
if (scaleinfo_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::ScaleInfo Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::VOLATILE_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
scaleinfo_reader_ = subscriber_->create_datareader(scaleinfo_topic_, reader_qos, this, StatusMask::all());
|
||||
if (scaleinfo_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::ScaleInfo DataReader initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
weightinfook_topic_ = participant_->create_topic("WeightInfoOk", weightinfook_type_.get_type_name(), topic_qos);
|
||||
if (weightinfook_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::WeightInfoOk Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
weightinfook_reader_ = subscriber_->create_datareader(weightinfook_topic_, reader_qos, this, StatusMask::all());
|
||||
if (weightinfook_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::WeightInfoOk DataReader initialization failed");
|
||||
}
|
||||
|
||||
// Create the topic
|
||||
topic_qos = TOPIC_QOS_DEFAULT;
|
||||
participant_->get_default_topic_qos(topic_qos);
|
||||
weightinfoerror_topic_ = participant_->create_topic("WeightInfoError", weightinfoerror_type_.get_type_name(), topic_qos);
|
||||
if (weightinfoerror_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::WeightInfoError Topic initialization failed");
|
||||
}
|
||||
|
||||
// Create the reader
|
||||
reader_qos = DATAREADER_QOS_DEFAULT;
|
||||
subscriber_->get_default_datareader_qos(reader_qos);
|
||||
reader_qos.reliability().kind = ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
reader_qos.reliability().max_blocking_time = Duration_t(1, 0);
|
||||
reader_qos.durability().kind = DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
|
||||
reader_qos.history().kind = HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
reader_qos.history().depth = 1;
|
||||
reader_qos.resource_limits().max_samples = 200;
|
||||
reader_qos.resource_limits().max_instances = 1;
|
||||
reader_qos.resource_limits().max_samples_per_instance = 100;
|
||||
reader_qos.data_sharing().off();
|
||||
weightinfoerror_reader_ = subscriber_->create_datareader(weightinfoerror_topic_, reader_qos, this, StatusMask::all());
|
||||
if (weightinfoerror_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::WeightInfoError DataReader initialization failed");
|
||||
}
|
||||
}
|
||||
|
||||
SubscriberApp::~SubscriberApp()
|
||||
{
|
||||
if (nullptr != participant_)
|
||||
{
|
||||
// Delete DDS entities contained within the DomainParticipant
|
||||
participant_->delete_contained_entities();
|
||||
|
||||
// Delete DomainParticipant
|
||||
factory_->delete_participant(participant_);
|
||||
}
|
||||
}
|
||||
|
||||
void SubscriberApp::on_subscription_matched(
|
||||
DataReader* reader,
|
||||
const SubscriptionMatchedStatus& info)
|
||||
{
|
||||
if (info.current_count_change == 1)
|
||||
{
|
||||
std::cout << reader->get_topicdescription()->get_name() << " Subscriber matched." << std::endl;
|
||||
}
|
||||
else if (info.current_count_change == -1)
|
||||
{
|
||||
std::cout << reader->get_topicdescription()->get_name() << " Subscriber unmatched." << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << info.current_count_change
|
||||
<< " is not a valid value for SubscriptionMatchedStatus current count change" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void SubscriberApp::on_data_available(
|
||||
DataReader* reader)
|
||||
{
|
||||
SampleInfo info;
|
||||
std::string topic_name = reader->get_topicdescription()->get_name();
|
||||
std::cout << topic_name << std::endl;
|
||||
|
||||
if (topic_name == "BarUpdate")
|
||||
{
|
||||
WeighingSystem::BarUpdate sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::BarUpdate_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "LightsUpdate")
|
||||
{
|
||||
WeighingSystem::LightsUpdate sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::LightsUpdate_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "InfraredUpdate")
|
||||
{
|
||||
WeighingSystem::InfraredUpdate sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::InfraredUpdate_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "InfraredCommandUpdate")
|
||||
{
|
||||
WeighingSystem::InfraredCommandUpdate sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::InfraredCommandUpdate_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "LicenseSnapUpdate")
|
||||
{
|
||||
WeighingSystem::LicenseSnapUpdate sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::LicenseSnapUpdate_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "ScaleInfo")
|
||||
{
|
||||
WeighingSystem::ScaleInfo sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::ScaleInfo_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "WeightInfoOk")
|
||||
{
|
||||
WeighingSystem::WeightInfoOk sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::WeightInfoOk_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "WeightInfoError")
|
||||
{
|
||||
WeighingSystem::WeightInfoError sample_;
|
||||
while ((!is_stopped()) && (RETCODE_OK == reader->take_next_sample(&sample_, &info)))
|
||||
{
|
||||
if ((info.instance_state == ALIVE_INSTANCE_STATE) && info.valid_data)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(DdsMsgData::queue_cv_mtx_);
|
||||
DdsMsgData::WeightInfoError_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SubscriberApp::run(std::shared_ptr<mqtt::async_client> dev)
|
||||
{
|
||||
while (!is_stopped())
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> period_lock(terminate_cv_mtx_);
|
||||
terminate_cv_.wait_for(period_lock, std::chrono::milliseconds(period_ms_), [this](){return is_stopped();});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool SubscriberApp::is_stopped()
|
||||
{
|
||||
return stop_.load();
|
||||
}
|
||||
|
||||
void SubscriberApp::stop()
|
||||
{
|
||||
stop_.store(true);
|
||||
terminate_cv_.notify_all();
|
||||
}
|
||||
@ -0,0 +1,98 @@
|
||||
// Copyright 2016 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.
|
||||
|
||||
/*!
|
||||
* @file SubscriberApp.hpp
|
||||
* This header file contains the declaration of the subscriber functions.
|
||||
*
|
||||
* This file was generated by the tool fastddsgen.
|
||||
*/
|
||||
|
||||
#ifndef FAST_DDS_GENERATED__SUBSCRIBERAPP_HPP
|
||||
#define FAST_DDS_GENERATED__SUBSCRIBERAPP_HPP
|
||||
|
||||
#include <condition_variable>
|
||||
|
||||
#include <fastdds/dds/domain/DomainParticipant.hpp>
|
||||
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
|
||||
#include <fastdds/dds/subscriber/DataReaderListener.hpp>
|
||||
#include <fastdds/dds/topic/TypeSupport.hpp>
|
||||
|
||||
#include "System.hpp"
|
||||
#include "mqtt/async_client.h"
|
||||
|
||||
class SubscriberApp : public eprosima::fastdds::dds::DataReaderListener
|
||||
{
|
||||
public:
|
||||
|
||||
SubscriberApp(
|
||||
const int& domain_id);
|
||||
|
||||
virtual ~SubscriberApp();
|
||||
|
||||
//! Subscription callback
|
||||
void on_data_available(
|
||||
eprosima::fastdds::dds::DataReader* reader) override;
|
||||
|
||||
//! Subscriber matched method
|
||||
void on_subscription_matched(
|
||||
eprosima::fastdds::dds::DataReader* reader,
|
||||
const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) override;
|
||||
|
||||
//! Run subscriber
|
||||
void run(std::shared_ptr<mqtt::async_client> dev);
|
||||
|
||||
//! Trigger the end of execution
|
||||
void stop();
|
||||
|
||||
private:
|
||||
|
||||
//! Return the current state of execution
|
||||
bool is_stopped();
|
||||
|
||||
std::shared_ptr<eprosima::fastdds::dds::DomainParticipantFactory> factory_;
|
||||
eprosima::fastdds::dds::DomainParticipant* participant_;
|
||||
eprosima::fastdds::dds::Subscriber* subscriber_;
|
||||
eprosima::fastdds::dds::Topic* bar_topic_;
|
||||
eprosima::fastdds::dds::DataReader* bar_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport bar_type_;
|
||||
eprosima::fastdds::dds::Topic* light_topic_;
|
||||
eprosima::fastdds::dds::DataReader* light_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport light_type_;
|
||||
eprosima::fastdds::dds::Topic* infraredinfo_topic_;
|
||||
eprosima::fastdds::dds::DataReader* infraredinfo_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport infraredinfo_type_;
|
||||
eprosima::fastdds::dds::Topic* infraredcommand_topic_;
|
||||
eprosima::fastdds::dds::DataReader* infraredcommand_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport infraredcommand_type_;
|
||||
eprosima::fastdds::dds::Topic* license_topic_;
|
||||
eprosima::fastdds::dds::DataReader* license_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport license_type_;
|
||||
eprosima::fastdds::dds::Topic* scaleinfo_topic_;
|
||||
eprosima::fastdds::dds::DataReader* scaleinfo_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport scaleinfo_type_;
|
||||
eprosima::fastdds::dds::Topic* weightinfook_topic_;
|
||||
eprosima::fastdds::dds::DataReader* weightinfook_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport weightinfook_type_;
|
||||
eprosima::fastdds::dds::Topic* weightinfoerror_topic_;
|
||||
eprosima::fastdds::dds::DataReader* weightinfoerror_reader_;
|
||||
eprosima::fastdds::dds::TypeSupport weightinfoerror_type_;
|
||||
uint16_t samples_received_;
|
||||
std::atomic<bool> stop_;
|
||||
uint32_t period_ms_ = 100; // in ms
|
||||
mutable std::mutex terminate_cv_mtx_;
|
||||
std::condition_variable terminate_cv_;
|
||||
};
|
||||
|
||||
#endif // FAST_DDS_GENERATED__SUBSCRIBERAPP_HPP
|
||||
@ -0,0 +1,249 @@
|
||||
// Copyright 2016 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.
|
||||
|
||||
/*!
|
||||
* @file main.cxx
|
||||
* This file acts as a main entry point to the application.
|
||||
*
|
||||
* This file was generated by the tool fastddsgen.
|
||||
*/
|
||||
|
||||
#include <csignal>
|
||||
#include <cstring>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
#include <fastdds/dds/log/Log.hpp>
|
||||
|
||||
#include "Subscriber.hpp"
|
||||
#include "Publisher.hpp"
|
||||
|
||||
#include "mqtt/async_client.h"
|
||||
#include "msg.hpp"
|
||||
|
||||
#define VERSION "v1.0"
|
||||
|
||||
using eprosima::fastdds::dds::Log;
|
||||
|
||||
std::function<void(int)> stop_handler;
|
||||
void signal_handler(
|
||||
int signum)
|
||||
{
|
||||
stop_handler(signum);
|
||||
}
|
||||
|
||||
std::string parse_signal(
|
||||
const int& signum)
|
||||
{
|
||||
switch (signum)
|
||||
{
|
||||
case SIGINT:
|
||||
return "SIGINT";
|
||||
case SIGTERM:
|
||||
return "SIGTERM";
|
||||
#ifndef _WIN32
|
||||
case SIGQUIT:
|
||||
return "SIGQUIT";
|
||||
case SIGHUP:
|
||||
return "SIGHUP";
|
||||
#endif // _WIN32
|
||||
default:
|
||||
return "UNKNOWN SIGNAL";
|
||||
}
|
||||
}
|
||||
|
||||
class mqtt_callback : public mqtt::callback
|
||||
{
|
||||
public:
|
||||
mqtt_callback(
|
||||
mqtt::async_client& cli,
|
||||
std::vector<std::string> topics,
|
||||
std::vector<int> qos)
|
||||
: cli_(cli)
|
||||
, sub_topic_(std::move(topics))
|
||||
, sub_qos_(std::move(qos))
|
||||
{}
|
||||
|
||||
void connected(const std::string &cause) override
|
||||
{
|
||||
std::cout << "[MQTT] Connected: " << cause << std::endl;
|
||||
for (size_t i = 0; i < sub_topic_.size(); ++i)
|
||||
{
|
||||
std::cout << "topic: " << sub_topic_[i] << std::endl;
|
||||
std::cout << "qos: " << sub_qos_[i] << std::endl;
|
||||
cli_.subscribe(sub_topic_[i], sub_qos_[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void connection_lost(const std::string &cause) override
|
||||
{
|
||||
std::cout << "[MQTT] Connection lost: " << cause << std::endl;
|
||||
}
|
||||
|
||||
void message_arrived(mqtt::const_message_ptr msg) override
|
||||
{
|
||||
std::cout << "[MQTT] "
|
||||
<< msg->get_topic()
|
||||
<< " -> "
|
||||
<< msg->to_string()
|
||||
<< std::endl;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(MqttMsgData::queue_cv_mtx_);
|
||||
MqttMsgData::Mqtt_msg_queue_.push(msg);
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
void delivery_complete(mqtt::delivery_token_ptr tok) override
|
||||
{
|
||||
// 发布消息完成
|
||||
}
|
||||
private:
|
||||
mqtt::async_client& cli_;
|
||||
std::vector<std::string> sub_topic_;
|
||||
std::vector<int> sub_qos_;
|
||||
};
|
||||
|
||||
std::queue<WeighingSystem::BarUpdate> DdsMsgData::BarUpdate_queue_;
|
||||
std::queue<WeighingSystem::LightsUpdate> DdsMsgData::LightsUpdate_queue_;
|
||||
std::queue<WeighingSystem::InfraredUpdate> DdsMsgData::InfraredUpdate_queue_;
|
||||
std::queue<WeighingSystem::InfraredCommandUpdate> DdsMsgData::InfraredCommandUpdate_queue_;
|
||||
std::queue<WeighingSystem::LicenseSnapUpdate> DdsMsgData::LicenseSnapUpdate_queue_;
|
||||
std::queue<WeighingSystem::ScaleInfo> DdsMsgData::ScaleInfo_queue_;
|
||||
std::queue<WeighingSystem::WeightInfoOk> DdsMsgData::WeightInfoOk_queue_;
|
||||
std::queue<WeighingSystem::WeightInfoError> DdsMsgData::WeightInfoError_queue_;
|
||||
std::mutex DdsMsgData::queue_cv_mtx_;
|
||||
std::queue<mqtt::const_message_ptr> MqttMsgData::Mqtt_msg_queue_;
|
||||
std::mutex MqttMsgData::queue_cv_mtx_;
|
||||
std::map<std::string, std::string> SubPubData::sub_to_pub_queue_;
|
||||
std::mutex SubPubData::queue_cv_mtx_;
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
auto ret = EXIT_SUCCESS;
|
||||
std::shared_ptr<SubscriberApp> sub;
|
||||
std::shared_ptr<PublisherApp> pub;
|
||||
std::shared_ptr<mqtt::async_client> dev = nullptr;
|
||||
|
||||
int domain_id = 0;
|
||||
std::string mqtt_server;
|
||||
std::string mqtt_username = "admin";
|
||||
std::string mqtt_password = "admin";
|
||||
std::string mqtt_id = "core_0";
|
||||
const std::vector<std::string> topic = {"command"};
|
||||
const std::vector<int> qos = {0};
|
||||
|
||||
for (int i = 1; i < argc; i++)
|
||||
{
|
||||
if (strcmp(argv[i], "--domain") == 0 && i + 1 < argc)
|
||||
{
|
||||
domain_id = atoi(argv[++i]);
|
||||
mqtt_id = std::string("core_") + std::string(argv[i]);
|
||||
}
|
||||
else if (strcmp(argv[i], "--addr") == 0 && i + 1 < argc)
|
||||
{
|
||||
mqtt_server = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "--user") == 0 && i + 1 < argc)
|
||||
{
|
||||
mqtt_username = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "--password") == 0 && i + 1 < argc)
|
||||
{
|
||||
mqtt_password = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "--version") == 0)
|
||||
{
|
||||
std::cout << "Vesrion: " << VERSION << "\n";
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
else if (strcmp(argv[i], "--help") == 0)
|
||||
{
|
||||
std::cout << "Usage: [options]\n"
|
||||
<< "Options:\n"
|
||||
<< " --domain Set domain ID\n"
|
||||
<< " --addr Set mqtt addr (e.g., mqtt://192.168.1.1:1883)\n"
|
||||
<< " --user Set mqtt username (e.g., admin)\n"
|
||||
<< " --password Set mqtt password (e.g., admin)\n"
|
||||
<< " --version Show software Version\n"
|
||||
<< " --help Show this help message\n";
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "Unknown option: " << argv[i] << "\n";
|
||||
std::cout << "Usage: [options]\n"
|
||||
<< "Options:\n"
|
||||
<< " --domain Set domain ID\n"
|
||||
<< " --addr Set mqtt addr (e.g., mqtt://192.168.1.1:1883)\n"
|
||||
<< " --user Set mqtt username (e.g., admin)\n"
|
||||
<< " --password Set mqtt password (e.g., admin)\n"
|
||||
<< " --version Show software Version\n"
|
||||
<< " --help Show this help message\n";
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
sub = std::make_shared<SubscriberApp>(domain_id);
|
||||
pub = std::make_shared<PublisherApp>(domain_id);
|
||||
|
||||
std::shared_ptr<mqtt_callback> cb;
|
||||
if (mqtt_server != "")
|
||||
{
|
||||
dev = std::make_shared<mqtt::async_client>(mqtt_server, mqtt_id);
|
||||
|
||||
auto connOpts = mqtt::connect_options_builder::v3()
|
||||
.user_name(mqtt_username)
|
||||
.password(mqtt_password)
|
||||
.keep_alive_interval(std::chrono::seconds(30))
|
||||
.automatic_reconnect(std::chrono::seconds(2), std::chrono::seconds(30))
|
||||
.clean_session(false)
|
||||
.finalize();
|
||||
|
||||
cb = std::make_shared<mqtt_callback>(*dev, topic, qos);
|
||||
dev->set_callback(*cb);
|
||||
|
||||
std::cout << "Connecting to broker..." << std::endl;
|
||||
dev->connect(connOpts);
|
||||
std::cout << "MQTT running..." << std::endl;
|
||||
}
|
||||
|
||||
std::thread sub_thread(&SubscriberApp::run, sub, dev);
|
||||
std::thread pub_thread(&PublisherApp::run, pub, dev);
|
||||
|
||||
std::cout << "Program is running. Please press Ctrl+C to stop at any time." << std::endl;
|
||||
|
||||
stop_handler = [&](int signum)
|
||||
{
|
||||
std::cout << "\n" << parse_signal(signum) << " received, stopping " << argv[1]
|
||||
<< " execution." << std::endl;
|
||||
sub->stop();
|
||||
pub->stop();
|
||||
};
|
||||
|
||||
signal(SIGINT, signal_handler);
|
||||
signal(SIGTERM, signal_handler);
|
||||
#ifndef _WIN32
|
||||
signal(SIGQUIT, signal_handler);
|
||||
signal(SIGHUP, signal_handler);
|
||||
#endif // _WIN32
|
||||
|
||||
sub_thread.join();
|
||||
pub_thread.join();
|
||||
|
||||
Log::Reset();
|
||||
return ret;
|
||||
}
|
||||
@ -0,0 +1,33 @@
|
||||
#ifndef _MSG_HPP_
|
||||
#define _MSG_HPP_
|
||||
|
||||
#include <queue>
|
||||
#include <mutex>
|
||||
#include "System.hpp"
|
||||
|
||||
class DdsMsgData {
|
||||
public:
|
||||
static std::queue<WeighingSystem::BarUpdate> BarUpdate_queue_;
|
||||
static std::queue<WeighingSystem::LightsUpdate> LightsUpdate_queue_;
|
||||
static std::queue<WeighingSystem::InfraredUpdate> InfraredUpdate_queue_;
|
||||
static std::queue<WeighingSystem::InfraredCommandUpdate> InfraredCommandUpdate_queue_;
|
||||
static std::queue<WeighingSystem::LicenseSnapUpdate> LicenseSnapUpdate_queue_;
|
||||
static std::queue<WeighingSystem::ScaleInfo> ScaleInfo_queue_;
|
||||
static std::queue<WeighingSystem::WeightInfoOk> WeightInfoOk_queue_;
|
||||
static std::queue<WeighingSystem::WeightInfoError> WeightInfoError_queue_;
|
||||
static std::mutex queue_cv_mtx_;
|
||||
};
|
||||
|
||||
class MqttMsgData {
|
||||
public:
|
||||
static std::queue<mqtt::const_message_ptr> Mqtt_msg_queue_;
|
||||
static std::mutex queue_cv_mtx_;
|
||||
};
|
||||
|
||||
class SubPubData {
|
||||
public:
|
||||
static std::map<std::string, std::string> sub_to_pub_queue_;
|
||||
static std::mutex queue_cv_mtx_;
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue