diff --git a/CMakeLists.txt b/CMakeLists.txt index 97bd46c..6abcc31 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,19 @@ target_link_libraries(System_lib fastcdr fastdds) # System_lib # ) +# GateCtrl Application. +add_executable(GateCtrl + gatectrl/main.cxx + gatectrl/Publisher.cxx + gatectrl/Subscriber.cxx + gatectrl/MsgHandler.cxx + common/CanMsgHandler.cxx + ) +target_include_directories(GateCtrl PRIVATE gatectrl) +target_link_libraries(GateCtrl fastcdr fastdds + System_lib + ) + # IoCtrl Application. add_executable(IoCtrl ioctrl/main.cxx diff --git a/gatectrl/MsgHandler.cxx b/gatectrl/MsgHandler.cxx new file mode 100644 index 0000000..0c99d5b --- /dev/null +++ b/gatectrl/MsgHandler.cxx @@ -0,0 +1,371 @@ +#include +#include +#include +#include +#include +#include +#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& msg) +{ + auto cmd = msg.find("cmd"); + + if (cmd != msg.end()) + { + if (cmd->second == "open") + { + if (this->device == "5serial") + { + std::vector data; + data.insert(data.end(), {this->device_id, 0x50, 1, 1}); + SendDeviceMsg(data); + } + } + else if (cmd->second == "write") + { + if (this->device == "5serial") + { + std::vector 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 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 data; + data.insert(data.end(), {this->device_id, 0x52, 1}); + data.insert(data.end(), 1); + SendDeviceMsg(data); + } + } + } +} + +bool MsgHandler::CtrlBar(const std::vector& port) +{ + std::vector 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& port) +{ + std::vector 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& 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 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& 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& data, int timeoutMs) +{ + return RecvMsg(data.data(), data.size(), timeoutMs); +} + diff --git a/gatectrl/MsgHandler.hpp b/gatectrl/MsgHandler.hpp new file mode 100644 index 0000000..6b8ae86 --- /dev/null +++ b/gatectrl/MsgHandler.hpp @@ -0,0 +1,66 @@ +#ifndef _MSGHANDLER_HPP_ +#define _MSGHANDLER_HPP_ + +#include +#include +#include +#include + +#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> omap; + std::map> 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& msg); + bool CtrlBar(const std::vector& port); + bool CtrlLight(const std::map& port); + int HandleDeviceMsg(); + + virtual bool OpenPort(const std::string& port, const std::string& baudrate); + virtual int SendDeviceMsg(std::vector& data); + virtual int RecvDeviceMsg(std::vector& 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& data); +}; + +#endif \ No newline at end of file diff --git a/gatectrl/Publisher.cxx b/gatectrl/Publisher.cxx new file mode 100644 index 0000000..e97c4d2 --- /dev/null +++ b/gatectrl/Publisher.cxx @@ -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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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 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 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 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 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 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(); +} \ No newline at end of file diff --git a/gatectrl/Publisher.hpp b/gatectrl/Publisher.hpp new file mode 100644 index 0000000..c454446 --- /dev/null +++ b/gatectrl/Publisher.hpp @@ -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 + +#include +#include +#include +#include + +#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 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 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 stop_; +}; + +#endif // FAST_DDS_GENERATED__PUBLISHERAPP_HPP \ No newline at end of file diff --git a/gatectrl/Subscriber.cxx b/gatectrl/Subscriber.cxx new file mode 100644 index 0000000..b591b72 --- /dev/null +++ b/gatectrl/Subscriber.cxx @@ -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 +#include + +#include +#include +#include +#include +#include +#include +#include + +#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 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 lock(MsgData::queue_cv_mtx_); + MsgData::LightsCommandUpdate_queue_.push(std::move(sample_)); + lock.unlock(); + } + } + } + } +} + +void SubscriberApp::run(std::shared_ptr handler) +{ + while (!is_stopped()) + { + std::unique_lock 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 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 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 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(); +} \ No newline at end of file diff --git a/gatectrl/Subscriber.hpp b/gatectrl/Subscriber.hpp new file mode 100644 index 0000000..e546122 --- /dev/null +++ b/gatectrl/Subscriber.hpp @@ -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 + +#include +#include +#include +#include + +#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 handler); + + //! Trigger the end of execution + void stop(); + +private: + + //! Return the current state of execution + bool is_stopped(); + + std::shared_ptr 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 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 \ No newline at end of file diff --git a/gatectrl/main.cxx b/gatectrl/main.cxx new file mode 100644 index 0000000..514171c --- /dev/null +++ b/gatectrl/main.cxx @@ -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 +#include +#include +#include +#include +#include + +#include + +#include "Subscriber.hpp" +#include "Publisher.hpp" +#include "CanMsgHandler.hpp" +#include "msg.hpp" + +#define VERSION "v1.0" + +using eprosima::fastdds::dds::Log; + +std::function 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 MsgData::BarCommandUpdate_queue_; +std::queue MsgData::LightsCommandUpdate_queue_; +std::mutex MsgData::queue_cv_mtx_; + +int main(int argc, char** argv) +{ + auto ret = EXIT_SUCCESS; + std::shared_ptr sub; + std::shared_ptr pub; + std::shared_ptr 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(domain_id); + pub = std::make_shared(domain_id); + dev = std::make_shared(); + + 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; +} diff --git a/gatectrl/msg.hpp b/gatectrl/msg.hpp new file mode 100644 index 0000000..e1c7a96 --- /dev/null +++ b/gatectrl/msg.hpp @@ -0,0 +1,15 @@ +#ifndef _MSG_HPP_ +#define _MSG_HPP_ + +#include +#include +#include "System.hpp" + +class MsgData { +public: + static std::queue BarCommandUpdate_queue_; + static std::queue LightsCommandUpdate_queue_; + static std::mutex queue_cv_mtx_; +}; + +#endif \ No newline at end of file