1. 新建道闸控制程序。
baocm
8 months ago
parent
e5a3c9f9d5
commit
3dbf090a91
@ -0,0 +1,371 @@
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <algorithm>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include "MsgHandler.hpp"
|
||||
|
||||
MsgHandler::MsgHandler() : fd(-1)
|
||||
{
|
||||
this->iomap.omap["FrontBarUp"] = {1, 1};
|
||||
this->iomap.omap["FrontBarDown"] = {2, 1};
|
||||
this->iomap.omap["BackBarUp"] = {3, 1};
|
||||
this->iomap.omap["BackBarDown"] = {4, 1};
|
||||
this->iomap.omap["FrontLED"] = {5, 1};
|
||||
this->iomap.omap["BackLED"] = {6, 1};
|
||||
|
||||
this->iomap.imap["FrontResistance"] = {5, 1};
|
||||
this->iomap.imap["BackResistance"] = {6, 1};
|
||||
|
||||
this->new_bar_state.FrontBarState = DOWN;
|
||||
this->new_bar_state.BackBarState = DOWN;
|
||||
this->new_bar_state.FrontLEDState = RED;
|
||||
this->new_bar_state.BackLEDState = RED;
|
||||
this->new_bar_state.FrontResistanceSignal = UNBLOCK;
|
||||
this->new_bar_state.BackResistanceSignal = UNBLOCK;
|
||||
|
||||
this->old_bar_state = this->new_bar_state;
|
||||
}
|
||||
|
||||
void MsgHandler::HandleDdsMsg(const std::map<std::string, std::string>& msg)
|
||||
{
|
||||
auto cmd = msg.find("cmd");
|
||||
|
||||
if (cmd != msg.end())
|
||||
{
|
||||
if (cmd->second == "open")
|
||||
{
|
||||
if (this->device == "5serial")
|
||||
{
|
||||
std::vector<uint8_t> data;
|
||||
data.insert(data.end(), {this->device_id, 0x50, 1, 1});
|
||||
SendDeviceMsg(data);
|
||||
}
|
||||
}
|
||||
else if (cmd->second == "write")
|
||||
{
|
||||
if (this->device == "5serial")
|
||||
{
|
||||
std::vector<uint8_t> data;
|
||||
data.resize(8, 1);
|
||||
data.insert(data.begin(), {this->device_id, 0x51, 1});
|
||||
for (const auto &port : this->iomap.omap)
|
||||
{
|
||||
data[port.second.first+2] = port.second.second;
|
||||
}
|
||||
|
||||
for (auto &m : msg)
|
||||
{
|
||||
auto port = this->iomap.omap.find(m.first);
|
||||
if (port != this->iomap.omap.end())
|
||||
{
|
||||
if (m.second == "1")
|
||||
{
|
||||
port->second.second = 1;
|
||||
}
|
||||
else if (m.second == "0")
|
||||
{
|
||||
port->second.second = 0;
|
||||
}
|
||||
data[port->second.first+2] = port->second.second;
|
||||
}
|
||||
}
|
||||
SendDeviceMsg(data);
|
||||
}
|
||||
}
|
||||
else if (cmd->second == "pluse")
|
||||
{
|
||||
if (this->device == "5serial")
|
||||
{
|
||||
std::vector<uint8_t> data;
|
||||
data.resize(8, 1);
|
||||
data.insert(data.begin(), {this->device_id, 0x51, 1});
|
||||
for (const auto &port : this->iomap.omap)
|
||||
{
|
||||
data[port.second.first+2] = port.second.second;
|
||||
}
|
||||
|
||||
for (auto &m : msg)
|
||||
{
|
||||
auto port = this->iomap.omap.find(m.first);
|
||||
if (port != this->iomap.omap.end())
|
||||
{
|
||||
if (m.second == "1")
|
||||
{
|
||||
port->second.second = 1;
|
||||
}
|
||||
else if (m.second == "0")
|
||||
{
|
||||
port->second.second = 0;
|
||||
}
|
||||
data[port->second.first+2] = port->second.second;
|
||||
}
|
||||
}
|
||||
SendDeviceMsg(data);
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
|
||||
for (auto &m : msg)
|
||||
{
|
||||
auto port = this->iomap.omap.find(m.first);
|
||||
if (port != this->iomap.omap.end())
|
||||
{
|
||||
if (m.second == "1")
|
||||
{
|
||||
port->second.second = 0;
|
||||
}
|
||||
else if (m.second == "0")
|
||||
{
|
||||
port->second.second = 1;
|
||||
}
|
||||
data[port->second.first+2] = port->second.second;
|
||||
}
|
||||
}
|
||||
SendDeviceMsg(data);
|
||||
}
|
||||
}
|
||||
else if (cmd->second == "read")
|
||||
{
|
||||
if (this->device == "5serial")
|
||||
{
|
||||
std::vector<uint8_t> data;
|
||||
data.insert(data.end(), {this->device_id, 0x52, 1});
|
||||
data.insert(data.end(), 1);
|
||||
SendDeviceMsg(data);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool MsgHandler::CtrlBar(const std::vector<std::string>& port)
|
||||
{
|
||||
std::vector<uint8_t> data;
|
||||
data.resize(8, 1);
|
||||
data.insert(data.begin(), {this->device_id, 0x51, 1});
|
||||
for (const auto &port : this->iomap.omap)
|
||||
{
|
||||
data[port.second.first + 2] = port.second.second;
|
||||
}
|
||||
|
||||
for (const std::string& p : port)
|
||||
{
|
||||
auto port = this->iomap.omap.find(p);
|
||||
if (port != this->iomap.omap.end())
|
||||
{
|
||||
port->second.second = 0;
|
||||
data[port->second.first + 2] = port->second.second;
|
||||
}
|
||||
}
|
||||
SendDeviceMsg(data);
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
|
||||
for (const std::string& p : port)
|
||||
{
|
||||
auto port = this->iomap.omap.find(p);
|
||||
if (port != this->iomap.omap.end())
|
||||
{
|
||||
port->second.second = 1;
|
||||
data[port->second.first + 2] = port->second.second;
|
||||
}
|
||||
}
|
||||
SendDeviceMsg(data);
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MsgHandler::CtrlLight(const std::map<std::string, uint8_t>& port)
|
||||
{
|
||||
std::vector<uint8_t> data;
|
||||
data.resize(8, 1);
|
||||
data.insert(data.begin(), {this->device_id, 0x51, 1});
|
||||
for (const auto &port : this->iomap.omap)
|
||||
{
|
||||
data[port.second.first + 2] = port.second.second;
|
||||
}
|
||||
|
||||
for (auto &m : port)
|
||||
{
|
||||
auto port = this->iomap.omap.find(m.first);
|
||||
if (port != this->iomap.omap.end())
|
||||
{
|
||||
if (m.second == 1)
|
||||
{
|
||||
port->second.second = 1;
|
||||
}
|
||||
else if (m.second == 2)
|
||||
{
|
||||
port->second.second = 0;
|
||||
}
|
||||
data[port->second.first + 2] = port->second.second;
|
||||
}
|
||||
}
|
||||
SendDeviceMsg(data);
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int MsgHandler::ParseDeviceMsg(std::vector<uint8_t>& data)
|
||||
{
|
||||
int ret = 0;
|
||||
if (data[0] == this->device_id)
|
||||
{
|
||||
switch(data[1])
|
||||
{
|
||||
case 0x50:
|
||||
std::cout << "can gpio open success" << std::endl;
|
||||
ret = 0;
|
||||
break;
|
||||
case 0x51:
|
||||
std::cout << "can gpio write success" << std::endl;
|
||||
break;
|
||||
ret = 0;
|
||||
case 0x52:
|
||||
for (auto &p : this->iomap.imap)
|
||||
{
|
||||
p.second.second = data[p.second.first+2];
|
||||
if (p.first == "FrontResistance")
|
||||
{
|
||||
if (p.second.second == 0)
|
||||
{
|
||||
this->new_bar_state.FrontResistanceSignal = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
this->new_bar_state.FrontResistanceSignal = 1;
|
||||
}
|
||||
}
|
||||
else if (p.first == "BackResistance")
|
||||
{
|
||||
if (p.second.second == 0)
|
||||
{
|
||||
this->new_bar_state.BackResistanceSignal = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
this->new_bar_state.BackResistanceSignal = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
ret = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
int MsgHandler::HandleDeviceMsg()
|
||||
{
|
||||
std::vector<uint8_t> m_RecvData;
|
||||
|
||||
if(RecvDeviceMsg(m_RecvData, 100) > 0)
|
||||
{
|
||||
for (int num : m_RecvData)
|
||||
{
|
||||
std::cout << std::hex << std::setw(2) << std::setfill('0')
|
||||
<< num << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
return(ParseDeviceMsg(m_RecvData));
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool MsgHandler::OpenPort(const std::string& port, const std::string& baudrate)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// 发送数据
|
||||
int MsgHandler::SendMsg(const void *buf, size_t len)
|
||||
{
|
||||
if (fd == -1) {
|
||||
perror("设备未打开");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int bytesWritten = write(fd, buf, len);
|
||||
if (bytesWritten < 0) {
|
||||
perror("发送数据失败");
|
||||
}
|
||||
|
||||
return bytesWritten;
|
||||
}
|
||||
|
||||
int MsgHandler::RecvMsg(void *buf, size_t len, int timeoutMs)
|
||||
{
|
||||
if (fd == -1) {
|
||||
perror("设备未打开");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 使用select实现超时
|
||||
fd_set readfds;
|
||||
struct timeval tv;
|
||||
|
||||
FD_ZERO(&readfds);
|
||||
FD_SET(fd, &readfds);
|
||||
|
||||
tv.tv_sec = timeoutMs / 1000;
|
||||
tv.tv_usec = (timeoutMs % 1000) * 1000;
|
||||
|
||||
int ret = select(fd + 1, &readfds, NULL, NULL, &tv);
|
||||
if (ret == -1) {
|
||||
perror("select错误");
|
||||
return -1;
|
||||
} else if (ret == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 有数据可读
|
||||
int bytes;
|
||||
if (len == 0)
|
||||
{
|
||||
if (ioctl(fd, FIONREAD, &bytes) < 0)
|
||||
{
|
||||
perror("ioctl FIONREAD失败");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
bytes = len;
|
||||
}
|
||||
|
||||
int bytesRead = read(fd, buf, bytes);
|
||||
if (bytesRead < 0) {
|
||||
perror("读取数据失败");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return bytesRead;
|
||||
}
|
||||
|
||||
void MsgHandler::ClosePort()
|
||||
{
|
||||
if (fd != -1)
|
||||
{
|
||||
close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
int MsgHandler::SendDeviceMsg(std::vector<uint8_t>& data)
|
||||
{
|
||||
for (int num : data) {
|
||||
std::cout << std::hex << std::setw(2) << std::setfill('0')
|
||||
<< num << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
return SendMsg(data.data(), data.size());
|
||||
}
|
||||
|
||||
int MsgHandler::RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs)
|
||||
{
|
||||
return RecvMsg(data.data(), data.size(), timeoutMs);
|
||||
}
|
||||
|
||||
@ -0,0 +1,66 @@
|
||||
#ifndef _MSGHANDLER_HPP_
|
||||
#define _MSGHANDLER_HPP_
|
||||
|
||||
#include <queue>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
#include "System.hpp"
|
||||
|
||||
#define UP 1
|
||||
#define DOWN 2
|
||||
#define RED 1
|
||||
#define GREEN 2
|
||||
#define UNBLOCK 0
|
||||
#define BLOCK 1
|
||||
|
||||
struct iomap_t
|
||||
{ //name, (port, value)
|
||||
std::map<std::string, std::pair<uint8_t, uint8_t>> omap;
|
||||
std::map<std::string, std::pair<uint8_t, uint8_t>> imap;
|
||||
};
|
||||
|
||||
struct bar_state_t
|
||||
{
|
||||
uint8_t FrontBarState;
|
||||
uint8_t BackBarState;
|
||||
uint8_t FrontLEDState;
|
||||
uint8_t BackLEDState;
|
||||
uint8_t FrontResistanceSignal;
|
||||
uint8_t BackResistanceSignal;
|
||||
};
|
||||
|
||||
class MsgHandler
|
||||
{
|
||||
public:
|
||||
int fd;
|
||||
std::string device;
|
||||
uint8_t device_id;
|
||||
iomap_t iomap;
|
||||
bar_state_t new_bar_state;
|
||||
bar_state_t old_bar_state;
|
||||
|
||||
MsgHandler();
|
||||
~MsgHandler() = default;
|
||||
|
||||
void HandleDdsMsg(const std::map<std::string, std::string>& msg);
|
||||
bool CtrlBar(const std::vector<std::string>& port);
|
||||
bool CtrlLight(const std::map<std::string, uint8_t>& port);
|
||||
int HandleDeviceMsg();
|
||||
|
||||
virtual bool OpenPort(const std::string& port, const std::string& baudrate);
|
||||
virtual int SendDeviceMsg(std::vector<uint8_t>& data);
|
||||
virtual int RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs);
|
||||
int SendMsg(const void *buf, size_t len);
|
||||
int RecvMsg(void *buf, size_t len, int timeoutMs);
|
||||
bool isOpen() const {
|
||||
return fd != -1;
|
||||
}
|
||||
void ClosePort();
|
||||
|
||||
private:
|
||||
int ParseDeviceMsg(std::vector<uint8_t>& data);
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,298 @@
|
||||
// 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 "msg.hpp"
|
||||
#include "MsgHandler.hpp"
|
||||
|
||||
using namespace eprosima::fastdds::dds;
|
||||
|
||||
PublisherApp::PublisherApp(
|
||||
const int& domain_id)
|
||||
: factory_(nullptr)
|
||||
, participant_(nullptr)
|
||||
, publisher_(nullptr)
|
||||
, bar_topic_(nullptr)
|
||||
, bar_writer_(nullptr)
|
||||
, bar_type_(new WeighingSystem::BarUpdatePubSubType())
|
||||
, light_topic_(nullptr)
|
||||
, light_writer_(nullptr)
|
||||
, light_type_(new WeighingSystem::LightsUpdatePubSubType())
|
||||
, infrared_topic_(nullptr)
|
||||
, infrared_writer_(nullptr)
|
||||
, infrared_type_(new WeighingSystem::LightsUpdatePubSubType())
|
||||
, matched_(0)
|
||||
, samples_sent_(0)
|
||||
, stop_(false)
|
||||
{
|
||||
//
|
||||
|
||||
// Create the participant
|
||||
DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT;
|
||||
pqos.name("WeighingSystem_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("WeighingSystem Participant initialization failed");
|
||||
}
|
||||
|
||||
// Register the type
|
||||
bar_type_.register_type(participant_);
|
||||
light_type_.register_type(participant_);
|
||||
infrared_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("WeighingSystem Publisher 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("BarUpdate 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::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();
|
||||
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("LightsUpdate", light_type_.get_type_name(), topic_qos);
|
||||
if (light_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("LightsUpdate 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();
|
||||
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);
|
||||
infrared_topic_ = participant_->create_topic("InfraredUpdate", infrared_type_.get_type_name(), topic_qos);
|
||||
if (infrared_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("InfraredUpdate 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();
|
||||
infrared_writer_ = publisher_->create_datawriter(infrared_topic_, writer_qos, this, StatusMask::all());
|
||||
if (infrared_writer_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::InfraredUpdate 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;
|
||||
}
|
||||
}
|
||||
|
||||
void PublisherApp::run(std::shared_ptr<MsgHandler> handler)
|
||||
{
|
||||
while (!is_stopped())
|
||||
{
|
||||
if(handler->isOpen() == true)
|
||||
{
|
||||
if(handler->HandleDeviceMsg())
|
||||
{
|
||||
// if ((handler->old_bar_state.FrontResistanceSignal != handler->new_bar_state.FrontResistanceSignal) ||
|
||||
// (handler->old_bar_state.BackResistanceSignal != handler->new_bar_state.BackResistanceSignal))
|
||||
// {
|
||||
WeighingSystem::InfraredUpdate info;
|
||||
info.FrontResistanceSignal() = handler->new_bar_state.FrontResistanceSignal;
|
||||
info.BackResistanceSignal() = handler->new_bar_state.BackResistanceSignal;
|
||||
handler->old_bar_state.FrontResistanceSignal = handler->new_bar_state.FrontResistanceSignal;
|
||||
handler->old_bar_state.BackResistanceSignal = handler->new_bar_state.BackResistanceSignal;
|
||||
|
||||
infrared_writer_->write(&info);
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
if ((handler->old_bar_state.FrontBarState != handler->new_bar_state.FrontBarState) || \
|
||||
(handler->old_bar_state.BackBarState != handler->new_bar_state.BackBarState)
|
||||
)
|
||||
{
|
||||
WeighingSystem::BarUpdate info;
|
||||
info.FrontBarState() = handler->new_bar_state.FrontBarState;
|
||||
info.BackBarState() = handler->new_bar_state.BackBarState;
|
||||
handler->old_bar_state.FrontBarState = handler->new_bar_state.FrontBarState;
|
||||
handler->old_bar_state.BackBarState = handler->new_bar_state.BackBarState;
|
||||
|
||||
bar_writer_->write(&info);
|
||||
}
|
||||
|
||||
if ((handler->old_bar_state.FrontLEDState != handler->new_bar_state.FrontLEDState) || \
|
||||
(handler->old_bar_state.BackLEDState != handler->new_bar_state.BackLEDState)
|
||||
)
|
||||
{
|
||||
WeighingSystem::LightsUpdate info;
|
||||
info.FrontLEDState() = handler->new_bar_state.FrontLEDState;
|
||||
info.BackLEDState() = handler->new_bar_state.BackLEDState;
|
||||
handler->old_bar_state.FrontLEDState = handler->new_bar_state.FrontLEDState;
|
||||
handler->old_bar_state.BackLEDState = handler->new_bar_state.BackLEDState;
|
||||
|
||||
light_writer_->write(&info);
|
||||
}
|
||||
|
||||
// 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::BarUpdate 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,82 @@
|
||||
// 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 "MsgHandler.hpp"
|
||||
|
||||
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<MsgHandler> handler);
|
||||
|
||||
//! Trigger the end of execution
|
||||
void stop();
|
||||
|
||||
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* infrared_topic_;
|
||||
eprosima::fastdds::dds::DataWriter* infrared_writer_;
|
||||
eprosima::fastdds::dds::TypeSupport infrared_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,286 @@
|
||||
// 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::BarCommandUpdatePubSubType())
|
||||
, light_topic_(nullptr)
|
||||
, light_reader_(nullptr)
|
||||
, light_type_(new WeighingSystem::LightsCommandUpdatePubSubType())
|
||||
, 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_);
|
||||
|
||||
// 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("BarCommandUpdate", bar_type_.get_type_name(), topic_qos);
|
||||
if (bar_topic_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::BarCommandUpdate 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::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();
|
||||
bar_reader_ = subscriber_->create_datareader(bar_topic_, reader_qos, this, StatusMask::all());
|
||||
if (bar_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::BarCommandUpdate DataReader 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("WeighingSystem::LightsCommandUpdate 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();
|
||||
light_reader_ = subscriber_->create_datareader(light_topic_, reader_qos, this, StatusMask::all());
|
||||
if (light_reader_ == nullptr)
|
||||
{
|
||||
throw std::runtime_error("WeighingSystem::LightsCommandUpdate 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 == "BarCommandUpdateTopic")
|
||||
{
|
||||
WeighingSystem::BarCommandUpdate 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(MsgData::queue_cv_mtx_);
|
||||
MsgData::BarCommandUpdate_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (topic_name == "LightsCommandUpdate")
|
||||
{
|
||||
WeighingSystem::LightsCommandUpdate 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(MsgData::queue_cv_mtx_);
|
||||
MsgData::LightsCommandUpdate_queue_.push(std::move(sample_));
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SubscriberApp::run(std::shared_ptr<MsgHandler> handler)
|
||||
{
|
||||
while (!is_stopped())
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(MsgData::queue_cv_mtx_, std::try_to_lock);
|
||||
if (lock.owns_lock())
|
||||
{
|
||||
if(!MsgData::BarCommandUpdate_queue_.empty())
|
||||
{
|
||||
WeighingSystem::BarCommandUpdate cmd = std::move(MsgData::BarCommandUpdate_queue_.front());
|
||||
MsgData::BarCommandUpdate_queue_.pop();
|
||||
lock.unlock();
|
||||
std::vector<std::string> port;
|
||||
if (cmd.FrontBarEnable() == true)
|
||||
{
|
||||
if (cmd.FrontBarSignalUp() == 1)
|
||||
{
|
||||
port.push_back("FrontBarUp");
|
||||
handler->new_bar_state.FrontBarState = 1;
|
||||
}
|
||||
else if (cmd.FrontBarSignalDown() == 1)
|
||||
{
|
||||
port.push_back("FrontBarDown");
|
||||
handler->new_bar_state.FrontBarState = 2;
|
||||
}
|
||||
}
|
||||
if (cmd.BackBarEnable() == true)
|
||||
{
|
||||
if (cmd.BackBarSignalUp() == 1)
|
||||
{
|
||||
port.push_back("BackBarUp");
|
||||
handler->new_bar_state.BackBarState = 1;
|
||||
}
|
||||
else if (cmd.BackBarSignalDown() == 1)
|
||||
{
|
||||
port.push_back("BackBarDown");
|
||||
handler->new_bar_state.BackBarState = 2;
|
||||
}
|
||||
}
|
||||
handler->CtrlBar(port);
|
||||
}
|
||||
else if(!MsgData::LightsCommandUpdate_queue_.empty())
|
||||
{
|
||||
WeighingSystem::LightsCommandUpdate cmd = std::move(MsgData::LightsCommandUpdate_queue_.front());
|
||||
MsgData::LightsCommandUpdate_queue_.pop();
|
||||
lock.unlock();
|
||||
std::map<std::string, uint8_t> port;
|
||||
if (cmd.FrontLEDEnable() == true)
|
||||
{
|
||||
port["FrontLED"] = cmd.FrontLEDSignal();
|
||||
handler->new_bar_state.FrontLEDState = cmd.FrontLEDSignal();
|
||||
}
|
||||
if (cmd.BackLEDEnable() == true)
|
||||
{
|
||||
port["BackLED"] = cmd.BackLEDSignal();
|
||||
handler->new_bar_state.BackLEDState = cmd.BackLEDSignal();
|
||||
}
|
||||
handler->CtrlLight(port);
|
||||
}
|
||||
else
|
||||
{
|
||||
lock.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
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,80 @@
|
||||
// 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 "MsgHandler.hpp"
|
||||
|
||||
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<MsgHandler> handler);
|
||||
|
||||
//! 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_;
|
||||
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,175 @@
|
||||
// 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 "CanMsgHandler.hpp"
|
||||
#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";
|
||||
}
|
||||
}
|
||||
|
||||
std::queue<WeighingSystem::BarCommandUpdate> MsgData::BarCommandUpdate_queue_;
|
||||
std::queue<WeighingSystem::LightsCommandUpdate> MsgData::LightsCommandUpdate_queue_;
|
||||
std::mutex MsgData::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<MsgHandler> dev;
|
||||
|
||||
int domain_id = 0;
|
||||
const char* interface = "can";
|
||||
const char* port = "can0";
|
||||
const char* baudrate = "500000";
|
||||
const char* device = "5serial";
|
||||
int device_id = 0;
|
||||
|
||||
for (int i = 1; i < argc; i++)
|
||||
{
|
||||
if (strcmp(argv[i], "--domain") == 0 && i + 1 < argc)
|
||||
{
|
||||
domain_id = atoi(argv[++i]);
|
||||
}
|
||||
else if (strcmp(argv[i], "--interface") == 0 && i + 1 < argc)
|
||||
{
|
||||
interface = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "--port") == 0 && i + 1 < argc)
|
||||
{
|
||||
port = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "--baudrate") == 0 && i + 1 < argc)
|
||||
{
|
||||
baudrate = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "--device") == 0 && i + 1 < argc)
|
||||
{
|
||||
device = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "--device_id") == 0 && i + 1 < argc)
|
||||
{
|
||||
device_id = atoi(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"
|
||||
<< " --interface Set interface (e.g., can)\n"
|
||||
<< " --baudrate Set baudrate (e.g., 9600)\n"
|
||||
<< " --port Set port name (e.g., can0)\n"
|
||||
<< " --device Set device name (e.g., 5serial)\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"
|
||||
<< " --interface Set interface (e.g., can)\n"
|
||||
<< " --baudrate Set baudrate (e.g., 9600)\n"
|
||||
<< " --port Set port name (e.g., can0)\n"
|
||||
<< " --device Set device name (e.g., 5serial)\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);
|
||||
dev = std::make_shared<CanMsgHandler>();
|
||||
|
||||
dev->OpenPort(port, baudrate);
|
||||
dev->device = device;
|
||||
dev->device_id = device_id;
|
||||
|
||||
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;
|
||||
pub->stop();
|
||||
};
|
||||
|
||||
signal(SIGINT, signal_handler);
|
||||
signal(SIGTERM, signal_handler);
|
||||
#ifndef _WIN32
|
||||
signal(SIGQUIT, signal_handler);
|
||||
signal(SIGHUP, signal_handler);
|
||||
#endif // _WIN32
|
||||
|
||||
pub_thread.join();
|
||||
|
||||
Log::Reset();
|
||||
return ret;
|
||||
}
|
||||
@ -0,0 +1,15 @@
|
||||
#ifndef _MSG_HPP_
|
||||
#define _MSG_HPP_
|
||||
|
||||
#include <queue>
|
||||
#include <mutex>
|
||||
#include "System.hpp"
|
||||
|
||||
class MsgData {
|
||||
public:
|
||||
static std::queue<WeighingSystem::BarCommandUpdate> BarCommandUpdate_queue_;
|
||||
static std::queue<WeighingSystem::LightsCommandUpdate> LightsCommandUpdate_queue_;
|
||||
static std::mutex queue_cv_mtx_;
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue