|
|
// 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 <fstream>
|
|
|
#include <filesystem>
|
|
|
#include <limits.h>
|
|
|
#include <unistd.h> // ← 加这行：open / close / write / read / usleep
|
|
|
#include <fcntl.h> // ← 加这行：O_WRONLY
|
|
|
|
|
|
#include <fastdds/dds/log/Log.hpp>
|
|
|
|
|
|
#include "Subscriber.hpp"
|
|
|
#include "CanMsgHandler.hpp"
|
|
|
#include "msg.hpp"
|
|
|
#include "json.hpp"
|
|
|
|
|
|
#include <chrono>
|
|
|
#include "WeighingDDSType.hpp"
|
|
|
|
|
|
using eprosima::fastdds::dds::Log;
|
|
|
using json = nlohmann::json;
|
|
|
|
|
|
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::mutex MsgData::queue_cv_mtx_;
|
|
|
|
|
|
std::filesystem::path getExeDir()
|
|
|
{
|
|
|
char buf[PATH_MAX] = {0};
|
|
|
ssize_t len = readlink("/proc/self/exe", buf, sizeof(buf)-1);
|
|
|
return std::filesystem::path(std::string(buf, len)).parent_path();
|
|
|
}
|
|
|
|
|
|
int main(int argc, char** argv)
|
|
|
{
|
|
|
auto ret = EXIT_SUCCESS;
|
|
|
json config;
|
|
|
std::shared_ptr<SubscriberApp> sub;
|
|
|
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;
|
|
|
|
|
|
bool test_front_bar = false;
|
|
|
std::thread test_thread;
|
|
|
|
|
|
auto exeDir = getExeDir();
|
|
|
auto configPath = exeDir / "config.json";
|
|
|
std::ifstream file(configPath);
|
|
|
|
|
|
file >> config;
|
|
|
file.close();
|
|
|
|
|
|
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], "--test_front_bar") == 0)
|
|
|
{
|
|
|
test_front_bar = true;
|
|
|
}
|
|
|
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: " << PROGRAM_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"
|
|
|
<< " --test_front_bar Test: raise front bar after 3s, lower after 5s\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"
|
|
|
<< " --test_front_bar Test: raise front bar after 3s, lower after 5s\n";
|
|
|
return EXIT_FAILURE;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
sub = std::make_shared<SubscriberApp>(domain_id);
|
|
|
dev = std::make_shared<CanMsgHandler>();
|
|
|
|
|
|
// ★ 先设 device / device_id / iomap
|
|
|
dev->device = device;
|
|
|
dev->device_id = device_id;
|
|
|
dev->iomap.imap = config["ioctrl"]["iomap"]["in"]
|
|
|
.get<std::map<std::string, std::pair<uint8_t, uint8_t>>>();
|
|
|
dev->iomap.omap = config["ioctrl"]["iomap"]["out"]
|
|
|
.get<std::map<std::string, std::pair<uint8_t, uint8_t>>>();
|
|
|
// ★ 最后才 OpenPort（这时 this->device == "gpio" 才生效）
|
|
|
dev->OpenPort(port, baudrate);
|
|
|
|
|
|
// ★ 当 device == "gpio" 时，强制 init 48/49（因为 CanMsgHandler::OpenPort 不会做）
|
|
|
if (dev->device == "gpio")
|
|
|
{
|
|
|
|
|
|
auto write_gpio = [](int n, int v) {
|
|
|
char path[64];
|
|
|
snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", n);
|
|
|
int fd = open(path, O_WRONLY);
|
|
|
if (fd >= 0) { write(fd, v ? "1" : "0", 1); close(fd); }
|
|
|
};
|
|
|
auto export_gpio = [](int n) {
|
|
|
int fd = open("/sys/class/gpio/export", O_WRONLY);
|
|
|
if (fd >= 0) { char b[8]; int l = snprintf(b, sizeof(b), "%d", n);
|
|
|
write(fd, b, l); close(fd); usleep(100000); }
|
|
|
};
|
|
|
auto dir_gpio = [](int n, const char* d) {
|
|
|
char path[64];
|
|
|
snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/direction", n);
|
|
|
int fd = open(path, O_WRONLY);
|
|
|
if (fd >= 0) { write(fd, d, strlen(d)); close(fd); }
|
|
|
};
|
|
|
export_gpio(48); dir_gpio(48, "out"); write_gpio(48, 0);
|
|
|
export_gpio(49); dir_gpio(49, "out"); write_gpio(49, 0);
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (test_front_bar)
|
|
|
{
|
|
|
test_thread = std::thread([dev]()
|
|
|
{
|
|
|
std::cout << "[test] sleep 3s, then raise front bar..." << std::endl;
|
|
|
std::this_thread::sleep_for(std::chrono::seconds(3));
|
|
|
|
|
|
WeighingSystem::BarCommandUpdate cmd_up;
|
|
|
cmd_up.FrontBarEnable() = 1;
|
|
|
cmd_up.FrontBarSignalUp() = 1;
|
|
|
cmd_up.FrontBarSignalDown()= 0;
|
|
|
cmd_up.BackBarEnable() = 0;
|
|
|
dev->HandleBarCommand(cmd_up);
|
|
|
|
|
|
std::cout << "[test] raise front bar done." << std::endl;
|
|
|
// 5 秒后降杆的代码整段删掉
|
|
|
});
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
std::thread sub_thread(&SubscriberApp::run, sub, 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(); // ← 新增
|
|
|
};
|
|
|
|
|
|
signal(SIGINT, signal_handler);
|
|
|
signal(SIGTERM, signal_handler);
|
|
|
#ifndef _WIN32
|
|
|
signal(SIGQUIT, signal_handler);
|
|
|
signal(SIGHUP, signal_handler);
|
|
|
#endif // _WIN32
|
|
|
|
|
|
sub_thread.join();
|
|
|
|
|
|
if (test_front_bar && test_thread.joinable())
|
|
|
{
|
|
|
test_thread.detach(); // 测试线程自己跑完，主线程不等
|
|
|
}
|
|
|
|
|
|
Log::Reset();
|
|
|
return ret;
|
|
|
}
|