231 lines
7.9 KiB
C++
231 lines
7.9 KiB
C++
#include "rclcpp/rclcpp.hpp"
|
||
#include "sweeper_interfaces/msg/rtk.hpp"
|
||
#include "sweeper_interfaces/msg/pl.hpp"
|
||
#include "sweeper_interfaces/msg/route.hpp"
|
||
#include "sweeper_interfaces/msg/task.hpp"
|
||
#include "json.h"
|
||
#include <pthread.h>
|
||
#include <iostream>
|
||
#include <fstream>
|
||
#include <unistd.h>
|
||
#include "pl.hpp"
|
||
#include <sys/socket.h>
|
||
#include <arpa/inet.h>
|
||
#include <unistd.h>
|
||
#include <cstring>
|
||
#include <chrono> // 新增：用于时间戳处理
|
||
|
||
using namespace std;
|
||
using namespace std::chrono; // 新增：时间戳命名空间
|
||
pthread_t pl_thread_t;
|
||
int is_start = 0;
|
||
|
||
static int sock = -1;
|
||
|
||
// 新增：全局变量记录最后接收RTK数据的时间戳
|
||
system_clock::time_point last_rtk_time;
|
||
// 新增：超时阈值（0.5秒）
|
||
const duration<double> rtk_timeout = duration<double>(0.5);
|
||
|
||
class pl_node : public rclcpp::Node
|
||
{
|
||
public:
|
||
pl_node(std::string name) : Node(name)
|
||
{
|
||
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
|
||
// 初始化最后接收时间为当前时间（避免初始状态直接超时）
|
||
last_rtk_time = system_clock::now();
|
||
|
||
// 创建订阅者订阅话题
|
||
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>("rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1));
|
||
task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>("task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1));
|
||
|
||
// 创建发布者
|
||
msg_publisher_ = this->create_publisher<sweeper_interfaces::msg::Pl>("pl_message", 10);
|
||
task_publisher_ = this->create_publisher<sweeper_interfaces::msg::Task>("task_message/upload", 10);
|
||
|
||
// 创建定时器，定时发布
|
||
timer_pl = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this));
|
||
timer_task = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this));
|
||
}
|
||
|
||
private:
|
||
// 收到话题数据的回调函数
|
||
void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg)
|
||
{
|
||
// 新增：更新最后接收时间戳
|
||
last_rtk_time = system_clock::now();
|
||
|
||
// RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'",
|
||
// msg->lat, msg->lon, msg->head, msg->speed, msg->p_quality, msg->h_quality);
|
||
|
||
g_rtk.reliability = 1;
|
||
if (msg->p_quality != 4 || msg->h_quality != 4)
|
||
{
|
||
g_rtk.reliability = 0;
|
||
}
|
||
|
||
g_rtk.lat = msg->lat;
|
||
g_rtk.lon = msg->lon;
|
||
g_rtk.direction = msg->head;
|
||
}
|
||
|
||
void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg)
|
||
{
|
||
if (is_start == 0 && msg->task_status == 1)
|
||
{
|
||
pthread_create(&pl_thread_t, NULL, pl_thread, NULL);
|
||
cout << " started" << endl;
|
||
is_start = 1;
|
||
task_status = TaskStatus::PENDING;
|
||
}
|
||
else if (is_start == 1 && msg->task_status == 0)
|
||
{
|
||
pthread_cancel(pl_thread_t);
|
||
cout << "pl_thread_t is canceled " << endl;
|
||
is_start = 0;
|
||
task_status = TaskStatus::COMPLETED;
|
||
}
|
||
// RCLCPP_INFO(this->get_logger(), " ==================================== is_start : %d", is_start);
|
||
// RCLCPP_INFO(this->get_logger(), " ==================================== task_status : %d", msg->task_status);
|
||
}
|
||
|
||
void timer_callback_pl()
|
||
{
|
||
// 新增：计算当前时间与最后接收时间的差值
|
||
auto now = system_clock::now();
|
||
duration<double> time_since_last = now - last_rtk_time;
|
||
|
||
if (is_start == 1)
|
||
{
|
||
if (sock < 0) // 首次连接
|
||
{
|
||
struct sockaddr_in serv_addr;
|
||
if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0)
|
||
{
|
||
RCLCPP_ERROR(this->get_logger(), "Socket creation error");
|
||
return;
|
||
}
|
||
|
||
serv_addr.sin_family = AF_INET;
|
||
serv_addr.sin_port = htons(9999);
|
||
|
||
if (inet_pton(AF_INET, "192.168.5.1", &serv_addr.sin_addr) <= 0)
|
||
{
|
||
RCLCPP_ERROR(this->get_logger(), "Invalid address/ Address not supported");
|
||
close(sock);
|
||
return;
|
||
}
|
||
|
||
if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
|
||
{
|
||
RCLCPP_ERROR(this->get_logger(), "Connection Failed");
|
||
close(sock);
|
||
sock = -1;
|
||
return;
|
||
}
|
||
}
|
||
|
||
const char *message = "Status: RUNNING";
|
||
if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接
|
||
{
|
||
close(sock);
|
||
sock = -1;
|
||
return timer_callback_pl(); // 重试
|
||
}
|
||
}
|
||
else if (sock >= 0)
|
||
{
|
||
close(sock); // 停止时关闭连接
|
||
sock = -1;
|
||
}
|
||
|
||
// 创建消息
|
||
sweeper_interfaces::msg::Pl message;
|
||
message.x = x;
|
||
message.y = y;
|
||
|
||
// 修改：同时判断RTK信号质量和超时情况
|
||
if (g_rtk.reliability == 0 || time_since_last > rtk_timeout)
|
||
{
|
||
message.speed = 0;
|
||
if (time_since_last > rtk_timeout)
|
||
{
|
||
RCLCPP_ERROR(this->get_logger(), "RTK数据超时(%.2fs)，停车！！！", time_since_last.count());
|
||
}
|
||
else
|
||
{
|
||
RCLCPP_ERROR(this->get_logger(), "RTK信号差，停车！！！");
|
||
}
|
||
}
|
||
else
|
||
message.speed = (float)pl_speed;
|
||
|
||
message.reliability = g_rtk.reliability;
|
||
// message.drive_mode = drive_mode;
|
||
if (drive_mode == Direction::STRAIGHT_D) // 直行
|
||
message.drive_mode = 0;
|
||
else if (drive_mode == Direction::LEFT_TURN || drive_mode == Direction::RIGHT_TURN) // 转弯
|
||
message.drive_mode = 1;
|
||
|
||
message.enter_range = nl_within_radius;
|
||
message.is_start = is_start;
|
||
// RCLCPP_INFO(this->get_logger(), " ==================================== message.is_start : %d", message.is_start);
|
||
|
||
// 日志打印
|
||
// RCLCPP_INFO(this->get_logger(), "x:'%lf',y:'%lf',speed:'%lf',drive_mode:'%d'",
|
||
// message.x, message.y, message.speed, message.drive_mode);
|
||
// 发布消息
|
||
msg_publisher_->publish(message);
|
||
}
|
||
|
||
void timer_callback_task()
|
||
{
|
||
// 创建消息
|
||
sweeper_interfaces::msg::Task message;
|
||
message.task_status = task_status;
|
||
task_publisher_->publish(message);
|
||
}
|
||
|
||
// 声名定时器指针
|
||
rclcpp::TimerBase::SharedPtr timer_pl;
|
||
rclcpp::TimerBase::SharedPtr timer_task;
|
||
|
||
// 声明话题发布者指针
|
||
rclcpp::Publisher<sweeper_interfaces::msg::Pl>::SharedPtr msg_publisher_;
|
||
rclcpp::Publisher<sweeper_interfaces::msg::Task>::SharedPtr task_publisher_;
|
||
|
||
// 声明订阅者
|
||
rclcpp::Subscription<sweeper_interfaces::msg::Task>::SharedPtr task_subscribe_;
|
||
rclcpp::Subscription<sweeper_interfaces::msg::Rtk>::SharedPtr msg_subscribe_;
|
||
};
|
||
|
||
void initConfig()
|
||
{
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
std::ifstream in("config.json", std::ios::binary);
|
||
if (!in.is_open())
|
||
{
|
||
std::cout << "read config file error" << std::endl;
|
||
return;
|
||
}
|
||
if (reader.parse(in, root))
|
||
{
|
||
nl_radius = root["detect_line_tolerance"].as<double>();
|
||
}
|
||
in.close(); // 关闭文件流
|
||
}
|
||
int main(int argc, char **argv)
|
||
{
|
||
initConfig();
|
||
rclcpp::init(argc, argv);
|
||
/*创建对应节点的共享指针对象*/
|
||
auto node = std::make_shared<pl_node>("pl_node");
|
||
/* 运行节点，并检测退出信号*/
|
||
rclcpp::spin(node);
|
||
rclcpp::shutdown();
|
||
|
||
// pthread_join(pl_thread_t, NULL);
|
||
return 0;
|
||
} |