cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-18 15:49:41

Browse Source
This commit is contained in:
cxh 2025-06-18 15:49:41 +08:00
parent 321ed6ea75
commit 624c7d1ea2
11 changed files with 270 additions and 281 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/mc/include/mc/can_utils.hpp Normal file
View File

@ -0,0 +1,14 @@
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "mc/can_struct.h"
extern bool g_can_print_enable;
struct CanHandlerContext
{
rclcpp::Node::SharedPtr node;
std::shared_ptr<rclcpp::Publisher<sweeper_interfaces::msg::CanFrame>> publisher;
};
void receiveHandler(const CANFrame &frame, void *userData);

20
src/mc/include/mc/control_cache.hpp Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <mutex>
#include <chrono>
class ControlCache
{
public:
void update(const sweeper_interfaces::msg::McCtrl &msg);
bool get(sweeper_interfaces::msg::McCtrl &msg);
private:
std::mutex mutex_;
sweeper_interfaces::msg::McCtrl latest_msg_;
std::chrono::steady_clock::time_point last_update_time_;
bool has_data_ = false;
};
extern ControlCache control_cache;
sweeper_interfaces::msg::McCtrl get_safe_control(); // 获取带超时判断的控制指令

3
src/mc/include/mc/dumper.h
View File

@ -32,7 +32,7 @@ public:
void tryStartup(); // 唤醒后自动执行复位（防重）
void updateDial(int new_dial, int current_state); // 拨钮变化检测与处理
void resetAwake(); // 清除唤醒状态（VCU 掉线）
void setSweeping(bool sweeping) { sweeping_ = sweeping; } // 外部设置扫地状态，
int getSimpleState() const;
private:
@ -53,6 +53,7 @@ private:
std::queue<DumperAction> action_queue_;
std::atomic<bool> busy_;
std::atomic<bool> has_pending_task_;
std::atomic<bool> sweeping_ = false; // 扫地任务标志
DumperState current_state_;
CANDriver &can_;
};

6
src/mc/include/mc/timer_tasks.hpp Normal file
View File

@ -0,0 +1,6 @@
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "mc/can_driver.h"
#include "mc/dumper.h"
void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl);

4
src/mc/include/mc/vcu_wakeup.hpp Normal file
View File

@ -0,0 +1,4 @@
#pragma once
#include "mc/can_struct.h"
void VCUWakeUp(CANDriver &driver); // 发送唤醒帧

39
src/mc/src/can_utils.cpp Normal file
View File

@ -0,0 +1,39 @@
#include "mc/can_utils.hpp"
#include <sstream>
#include <iomanip>
bool g_can_print_enable = false;
extern std::atomic<bool> vcu_msg_received;
extern std::atomic<bool> vcu_awake;
extern rclcpp::Time last_vcu_msg_time;
void receiveHandler(const CANFrame &frame, void *userData)
{
auto *context = static_cast<CanHandlerContext *>(userData);
auto node = context->node;
auto pub = context->publisher;
auto now = node->now();
sweeper_interfaces::msg::CanFrame msg;
msg.id = frame.id;
msg.dlc = frame.dlc;
msg.data.assign(frame.data, frame.data + frame.dlc);
pub->publish(msg);
if (g_can_print_enable)
{
std::stringstream ss;
ss << "CAN ID: " << std::hex << std::uppercase
<< std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') << frame.id << " Data: ";
for (int i = 0; i < frame.dlc; ++i)
ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
}
if (frame.id == 0x18FA0121)
{
last_vcu_msg_time = now;
vcu_msg_received.store(true);
vcu_awake.store(true);
}
}

44
src/mc/src/control_cache.cpp Normal file
View File

@ -0,0 +1,44 @@
#include "mc/control_cache.hpp"
ControlCache control_cache;
void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg)
{
std::lock_guard<std::mutex> lock(mutex_);
latest_msg_ = msg;
last_update_time_ = std::chrono::steady_clock::now();
has_data_ = true;
}
bool ControlCache::get(sweeper_interfaces::msg::McCtrl &msg)
{
std::lock_guard<std::mutex> lock(mutex_);
if (!has_data_)
return false;
auto now = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time_).count() > 100)
return false;
msg = latest_msg_;
return true;
}
sweeper_interfaces::msg::McCtrl get_safe_control()
{
sweeper_interfaces::msg::McCtrl msg;
if (!control_cache.get(msg))
{
msg.brake = 1;
msg.gear = 0;
msg.rpm = 0;
msg.angle = 0;
msg.angle_speed = 120;
msg.edge_brush_lift = false;
msg.sweep_ctrl = false;
msg.spray = false;
msg.mud_flap = false;
msg.dust_shake = false;
}
return msg;
}

8
src/mc/src/dumper.cpp
View File

@ -124,7 +124,13 @@ void DumperController::runActionSequence(const std::vector<DumperAction> &action
// 每个动作的具体 CAN 指令 + 延时
void DumperController::doAction(DumperAction act)
{
constexpr int delay_ms = 10000; // 可统一调整延时时间（单位：毫秒）
if (sweeping_)
{
std::cout << "[Warning] Sweeping active, dumper action blocked.\n";
return;
}
constexpr int delay_ms = 15000; // 可统一调整延时时间（单位：毫秒）
using namespace std::chrono_literals;
CANFrame frame1;

295
src/mc/src/mc.cpp
View File

@ -1,283 +1,36 @@
#include <cstdio>
#include <iostream>
#include <iomanip>
#include "mc/can_driver.h"
#include "mc/can_struct.h"
#include "mc/get_config.h"
#include "mc/dumper.h"
#include "rclcpp/rclcpp.hpp"
#include "mc/get_config.h"
#include "mc/can_driver.h"
#include "mc/dumper.h"
#include "mc/can_utils.hpp"
#include "mc/control_cache.hpp"
#include "mc/timer_tasks.hpp"
#include "mc/vcu_wakeup.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
// 全局开关，控制是否打印 CAN 消息到终端
bool g_can_print_enable = false;
namespace sweeperMsg = sweeper_interfaces::msg;
CANDriver canctl; // can驱动实例
DumperController dumper(canctl); // 斗控制器实例
struct CanHandlerContext
{
rclcpp::Node::SharedPtr node;
std::shared_ptr<rclcpp::Publisher<sweeperMsg::CanFrame>> publisher;
};
std::atomic<bool> vcu_msg_received = false; // 是否收到过vcu数据帧
std::atomic<bool> vcu_awake = false; // 是否唤醒成功
rclcpp::Time last_vcu_msg_time; // 上次收到vcu数据帧的时间
struct ControlCache
{
std::mutex mutex; // 防止多线程冲突
sweeperMsg::McCtrl latest_msg;
std::chrono::steady_clock::time_point last_update_time;
bool has_data = false;
void update(const sweeperMsg::McCtrl &msg)
{
std::lock_guard<std::mutex> lock(mutex);
latest_msg = msg;
last_update_time = std::chrono::steady_clock::now();
has_data = true;
}
bool get(sweeperMsg::McCtrl &msg)
{
std::lock_guard<std::mutex> lock(mutex);
if (!has_data)
return false;
auto now = std::chrono::steady_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time);
if (duration.count() > 100) // ms超时
{
return false; // 数据过期
}
msg = latest_msg;
return true;
}
};
ControlCache control_cache;
// 发布检测
sweeperMsg::McCtrl get_safe_control()
{
sweeperMsg::McCtrl msg;
if (!control_cache.get(msg))
{
// 超时或未接收到控制数据，进入安全状态
msg.brake = 1;
msg.gear = 0;
msg.rpm = 0;
msg.angle = 0;
msg.angle_speed = 120;
msg.edge_brush_lift = false;
msg.sweep_ctrl = false;
msg.spray = false;
msg.mud_flap = false;
msg.dust_shake = false;
}
return msg;
}
// CAN消息接收回调
void receiveHandler(const CANFrame &frame, void *userData)
{
// 获取 ROS 2 Publisher
auto context = static_cast<CanHandlerContext *>(userData);
auto publisher = context->publisher;
auto node = context->node;
auto now = node->now();
// 创建并发布 CAN 消息
auto msg = sweeperMsg::CanFrame();
msg.id = frame.id;
msg.dlc = frame.dlc;
msg.data.assign(frame.data, frame.data + frame.dlc);
publisher->publish(msg);
// 根据开关决定是否打印 CAN 消息
if (g_can_print_enable)
{
std::stringstream ss;
ss << "CAN ID: ";
if (frame.id > 0x7FF)
ss << std::hex << std::uppercase << frame.id;
else
ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ') << frame.id;
ss << " Data: ";
for (int i = 0; i < frame.dlc; ++i)
{
ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
}
RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
}
// VCU报文检查
if (frame.id == 0x18FA0121)
{
last_vcu_msg_time = now; // 更新时间
vcu_msg_received.store(true);
vcu_awake.store(true); // 唤醒成功
}
}
CANDriver canctl;
DumperController dumper(canctl);
std::atomic<bool> vcu_msg_received = false;
std::atomic<bool> vcu_awake = false;
rclcpp::Time last_vcu_msg_time;
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
{
dumper.setSweeping(msg->sweep_ctrl);
control_cache.update(*msg);
}
void VCUWakeUp()
{
CANFrame frame;
// 公共设置
frame.id = 0x18FF8017;
frame.dlc = 8;
frame.ext = true; // 使用扩展帧
frame.rtr = false; // 不是远程帧
// 第1帧：握手帧
frame.data[0] = 0x55;
frame.data[1] = 0x21;
for (int i = 2; i < 8; ++i)
frame.data[i] = 0xFF;
canctl.sendFrame(frame);
// 延时，例如 50ms
std::this_thread::sleep_for(std::chrono::milliseconds(50));
// 第2帧：启动帧
frame.data[0] = 0xAA;
frame.data[1] = 0x21;
for (int i = 2; i < 8; ++i)
frame.data[i] = 0xFF;
canctl.sendFrame(frame);
}
void setupTimers(rclcpp::Node::SharedPtr node)
{
// VCU 唤醒帧发送定时器，1Hz
static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
std::chrono::seconds(1), [node]()
{
if (!vcu_awake.load())
{
RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp();
} });
// vcu报文 watchdog 检查，200ms
static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
std::chrono::milliseconds(200), [node]()
{
auto now = node->now();
auto elapsed = now - last_vcu_msg_time;
if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
{
RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state.");
vcu_msg_received.store(false);
vcu_awake.store(false);
dumper.resetAwake();
}
if (vcu_awake.load())
{
dumper.tryStartup();
sweeperMsg::McCtrl msg = get_safe_control();
int to=static_cast<int>(msg.dump);
int from =dumper.getSimpleState();
dumper.updateDial(to, from);
} });
// DBS控制，100Hz
static rclcpp::TimerBase::SharedPtr timer_dbs = node->create_wall_timer(
std::chrono::milliseconds(10), []()
{
sweeperMsg::McCtrl msg = get_safe_control();
dbs_cmd.setEnabled(msg.ehb_anable);
dbs_cmd.setPressure(msg.ehb_brake_pressure);
canctl.sendFrame(dbs_cmd.toFrame()); });
// MCU控制，50Hz
static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
std::chrono::milliseconds(20), []()
{
sweeperMsg::McCtrl msg = get_safe_control();
mcu_cmd.setEnabled(true);
mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame()); });
// EPS 控制，20Hz
static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50), []()
{
sweeperMsg::McCtrl msg = get_safe_control();
eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame()); });
// VCU 控制，10Hz
static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100), []()
{
sweeperMsg::McCtrl msg = get_safe_control();
vcu1_cmd.setLeftLight(msg.left_light);
vcu1_cmd.setDustShake(msg.dust_shake);
vcu1_cmd.setHeadLight(msg.headlight);
vcu2_cmd.setBrakeLight(msg.brake_light);
vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
vcu2_cmd.setHeadlight(msg.headlight);
vcu2_cmd.setMudFlap(msg.mud_flap);
vcu2_cmd.setRightLight(msg.right_light);
vcu2_cmd.setSpray(msg.spray);
vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
// canctl.sendFrame(vcu1_cmd.toFrame());
// canctl.sendFrame(vcu2_cmd.toFrame());
});
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv); // 初始化 ROS 2
// 创建一个 ROS 2 节点
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("can_driver_node");
RCLCPP_INFO(node->get_logger(), "Starting mc package...");
// 创建一个 Publisher
auto can_publisher = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
// 创建 Subscriber（收控制指令）
auto subscriber = node->create_subscription<sweeperMsg::McCtrl>(
"mc_ctrl", // 订阅的 topic 名字
10, // 队列长度
mcCtrlCallback // 收到消息后的回调
);
auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
Config mc_config;
@ -289,24 +42,16 @@ int main(int argc, char **argv)
return -1;
}
// 创建上下文
auto context = std::make_shared<CanHandlerContext>();
context->node = node;
context->publisher = can_publisher;
context->publisher = pub;
canctl.setReceiveCallback(receiveHandler, context.get());
// 设置接收回调并传递上下文
canctl.setReceiveCallback(receiveHandler, (void *)context.get());
setupTimers(node, dumper, canctl);
// 添加定时器设置
setupTimers(node);
// 在 ROS shutdown 时自动清理
rclcpp::on_shutdown([&]()
{ canctl.close(); });
// 事件循环
rclcpp::spin(node);
rclcpp::shutdown(); // 关闭 ROS 2
rclcpp::shutdown();
return 0;
}

89
src/mc/src/timer_tasks.cpp Normal file
View File

@ -0,0 +1,89 @@
#include "mc/timer_tasks.hpp"
#include "mc/control_cache.hpp"
#include "mc/vcu_wakeup.hpp"
#include "mc/can_struct.h"
extern std::atomic<bool> vcu_awake;
extern std::atomic<bool> vcu_msg_received;
extern rclcpp::Time last_vcu_msg_time;
void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl)
{
static auto timer_wakeup = node->create_wall_timer(
std::chrono::seconds(1), [node, &canctl]()
{
if (!vcu_awake.load())
{
RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp(canctl);
} });
static auto timer_watchdog = node->create_wall_timer(
std::chrono::milliseconds(200), [node, &dumper]()
{
auto now = node->now();
if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5)))
{
RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state.");
vcu_awake.store(false);
vcu_msg_received.store(false);
dumper.resetAwake();
}
if (vcu_awake.load())
{
dumper.tryStartup();
auto msg = get_safe_control();
dumper.updateDial(static_cast<int>(msg.dump), dumper.getSimpleState());
} });
static auto timer_dbs = node->create_wall_timer(
std::chrono::milliseconds(10), [&canctl]()
{
auto msg = get_safe_control();
dbs_cmd.setEnabled(msg.ehb_anable);
dbs_cmd.setPressure(msg.ehb_brake_pressure);
canctl.sendFrame(dbs_cmd.toFrame()); });
static auto timer_mcu = node->create_wall_timer(
std::chrono::milliseconds(20), [&canctl]()
{
auto msg = get_safe_control();
mcu_cmd.setEnabled(true);
mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame()); });
static auto timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50), [&canctl]()
{
auto msg = get_safe_control();
eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame()); });
static auto timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100), [&canctl, &dumper]()
{
auto msg = get_safe_control();
vcu1_cmd.setLeftLight(msg.left_light);
vcu1_cmd.setDustShake(msg.dust_shake);
vcu1_cmd.setHeadLight(msg.headlight);
vcu2_cmd.setBrakeLight(msg.brake_light);
vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
vcu2_cmd.setHeadlight(msg.headlight);
vcu2_cmd.setMudFlap(msg.mud_flap);
vcu2_cmd.setRightLight(msg.right_light);
vcu2_cmd.setSpray(msg.spray);
vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
if (msg.sweep_ctrl && dumper.getSimpleState() == 0)
{
canctl.sendFrame(vcu1_cmd.toFrame());
canctl.sendFrame(vcu2_cmd.toFrame());
} });
}

21
src/mc/src/vcu_wakeup.cpp Normal file
View File

@ -0,0 +1,21 @@
#include "mc/vcu_wakeup.hpp"
#include <thread>
void VCUWakeUp(CANDriver &driver)
{
CANFrame frame;
frame.id = 0x18FF8017;
frame.dlc = 8;
frame.ext = true;
frame.rtr = false;
frame.data[0] = 0x55;
frame.data[1] = 0x21;
std::fill(frame.data + 2, frame.data + 8, 0xFF);
driver.sendFrame(frame);
std::this_thread::sleep_for(std::chrono::milliseconds(50));
frame.data[0] = 0xAA;
driver.sendFrame(frame);
}