1、判断RTK信号质量和超时情况
2、修改档位和速度，适配新200底盘
This commit is contained in:
parent
698758baa7
commit
dea1c5c797
@ -217,10 +217,10 @@ private:
|
||||
double x = 0.0;
|
||||
double y = 0.0;
|
||||
int speed = 0; // 从PL消息获取的速度
|
||||
int reliability = 0;
|
||||
int reliability = 0; // RTK信号质量 1行 0不行
|
||||
|
||||
// 发布消息的参数
|
||||
int publish_speed = 0; // 转速占空比 0-100
|
||||
int publish_speed = 0; // 转速 0-2000
|
||||
int publish_angle = 0; // [-66.0,66.0]，负数表示左转，正数表示右转
|
||||
|
||||
// 车体矩形边界（栅格坐标）
|
||||
@ -677,15 +677,22 @@ private:
|
||||
// 发布控制指令
|
||||
sweeper_interfaces::msg::McCtrl message;
|
||||
message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫
|
||||
message.gear = 0;
|
||||
message.gear = 2; // 前进挡
|
||||
message.brake = (publish_speed == 0) ? 1 : 0;
|
||||
message.rpm = publish_speed * 25;
|
||||
|
||||
if (reliability == 1)
|
||||
message.rpm = publish_speed;
|
||||
else
|
||||
{
|
||||
message.rpm = 0;
|
||||
RCLCPP_ERROR(this->get_logger(), "rtk信号差,停车！！！");
|
||||
}
|
||||
message.angle = publish_angle; // 负数表示左转，正数表示右转
|
||||
message.angle_speed = 800;
|
||||
|
||||
pub_mc->publish(message);
|
||||
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed * 30 << ", 角度=" << publish_angle << ", 状态=" << [&]()
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed << ", 角度=" << publish_angle << ", 状态=" << [&]()
|
||||
{
|
||||
switch(avoid_state_){
|
||||
case AVOID_NONE: return "无避障";
|
||||
@ -711,7 +718,7 @@ private:
|
||||
float target_angle = calculate_the_angle(x, y);
|
||||
|
||||
// 初始化控制参数
|
||||
publish_speed = std::max(15, std::min(speed, 30)); // 限制速度范围
|
||||
publish_speed = std::max(800, std::min(speed, 1500)); // 限制速度范围
|
||||
publish_angle = static_cast<int>(target_angle);
|
||||
|
||||
// 安全检查：未检测到车体位置
|
||||
@ -885,7 +892,7 @@ private:
|
||||
else
|
||||
{
|
||||
// 距离足够，可以减速接近
|
||||
publish_speed = std::max(10, publish_speed / 2);
|
||||
publish_speed = std::max(800, publish_speed / 2);
|
||||
RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格",
|
||||
obstacle_analysis_.obstacle_distance);
|
||||
}
|
||||
@ -1594,7 +1601,7 @@ private:
|
||||
double return_time = (current_time - avoid_ctx_.last_state_change_time).seconds();
|
||||
|
||||
// 恢复正常速度
|
||||
publish_speed = std::min(25, speed);
|
||||
publish_speed = std::min(1500, speed);
|
||||
|
||||
// 计算目标角度
|
||||
float target_angle = calculate_the_angle(x, y);
|
||||
|
||||
@ -267,7 +267,7 @@ void PL_ProcThread()
|
||||
|
||||
x = x_cm_tmp;
|
||||
y = y_cm_tmp;
|
||||
pl_speed = 80;
|
||||
pl_speed = 1500;
|
||||
task_status = TaskStatus::RUNNING;
|
||||
}
|
||||
|
||||
|
||||
@ -13,19 +13,29 @@
|
||||
#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.2);
|
||||
|
||||
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));
|
||||
@ -43,6 +53,9 @@ 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);
|
||||
|
||||
@ -50,7 +63,6 @@ private:
|
||||
if (msg->p_quality != 4 || msg->h_quality != 4)
|
||||
{
|
||||
g_rtk.reliability = 0;
|
||||
RCLCPP_INFO(this->get_logger(), "rtk信号差!");
|
||||
}
|
||||
|
||||
g_rtk.lat = msg->lat;
|
||||
@ -80,6 +92,10 @@ private:
|
||||
|
||||
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) // 首次连接
|
||||
@ -128,7 +144,23 @@ private:
|
||||
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) // 直行
|
||||
|
||||
Loading…
Reference in New Issue
Block a user