From 80201e18a948a3889e3cf451a34e26ab1a2217fa Mon Sep 17 00:00:00 2001 From: cxh Date: Wed, 11 Jun 2025 16:29:14 +0800 Subject: [PATCH] Auto commit at 2025-06-11 16:29:14 --- src/ctrl_arbiter/CMakeLists.txt | 4 +- src/ctrl_arbiter/package.xml | 5 +- src/ctrl_arbiter/src/ctrl_arbiter.cpp | 32 +- src/mc/CMakeLists.txt | 22 +- src/mc/include/mc/can_driver.h | 13 +- src/mc/include/mc/can_struct.h | 293 ++++-------------- src/mc/msg/McCtrl.msg | 31 -- src/mc/package.xml | 12 +- src/mc/src/can_driver.cpp | 108 +++---- src/mc/src/can_struct.cpp | 2 - src/mc/src/mc.cpp | 140 +++------ src/mqtt_report/CMakeLists.txt | 4 +- src/mqtt_report/package.xml | 2 +- src/mqtt_report/src/mqtt_report.cpp | 12 +- src/radio_ctrl/CMakeLists.txt | 7 +- src/radio_ctrl/config/config.json | 6 +- src/radio_ctrl/package.xml | 4 +- src/radio_ctrl/src/radio_ctrl.cpp | 66 ++-- src/sweeper_interfaces/CMakeLists.txt | 39 +++ .../msg/CanFrame.msg | 0 src/sweeper_interfaces/msg/McCtrl.msg | 26 ++ src/sweeper_interfaces/package.xml | 23 ++ 22 files changed, 277 insertions(+), 574 deletions(-) delete mode 100644 src/mc/msg/McCtrl.msg create mode 100644 src/sweeper_interfaces/CMakeLists.txt rename src/{mc => sweeper_interfaces}/msg/CanFrame.msg (100%) create mode 100644 src/sweeper_interfaces/msg/McCtrl.msg create mode 100644 src/sweeper_interfaces/package.xml diff --git a/src/ctrl_arbiter/CMakeLists.txt b/src/ctrl_arbiter/CMakeLists.txt index 0fac0db..64faf2e 100644 --- a/src/ctrl_arbiter/CMakeLists.txt +++ b/src/ctrl_arbiter/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(mc REQUIRED) +find_package(sweeper_interfaces REQUIRED) find_package(std_msgs REQUIRED) file(GLOB SRC_FILES "src/*.cpp") @@ -17,7 +17,7 @@ add_executable(ctrl_arbiter_node ${SRC_FILES}) ament_target_dependencies(ctrl_arbiter_node rclcpp - mc + sweeper_interfaces std_msgs ) diff --git a/src/ctrl_arbiter/package.xml b/src/ctrl_arbiter/package.xml index 2b3e914..725c7a8 100644 --- a/src/ctrl_arbiter/package.xml +++ b/src/ctrl_arbiter/package.xml @@ -9,10 +9,9 @@ ament_cmake + sweeper_interfaces rclcpp - mc - std_msgs - + ament_lint_auto ament_lint_common diff --git a/src/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/ctrl_arbiter/src/ctrl_arbiter.cpp index 11785b6..0d3e674 100644 --- a/src/ctrl_arbiter/src/ctrl_arbiter.cpp +++ b/src/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -1,9 +1,10 @@ #include "rclcpp/rclcpp.hpp" -#include "mc/msg/mc_ctrl.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" #include #include using namespace std::chrono_literals; +namespace sweeperMsg = sweeper_interfaces::msg; class ArbitrationNode : public rclcpp::Node { @@ -13,11 +14,11 @@ public: { timeout_ms_ = 200; // 200毫秒超时阈值，可调整 - publisher_ = this->create_publisher("/mc_ctrl", 10); + publisher_ = this->create_publisher("/mc_ctrl", 10); - sub_radio_ = this->create_subscription( + sub_radio_ = this->create_subscription( "/radio_mc_ctrl", 10, - [this](const mc::msg::McCtrl::SharedPtr msg) + [this](const sweeperMsg::McCtrl::SharedPtr msg) { std::lock_guard lock(mutex_); radio_msg_ = *msg; @@ -25,9 +26,9 @@ public: radio_valid_ = true; }); - sub_remote_ = this->create_subscription( + sub_remote_ = this->create_subscription( "/remote_mc_ctrl", 10, - [this](const mc::msg::McCtrl::SharedPtr msg) + [this](const sweeperMsg::McCtrl::SharedPtr msg) { std::lock_guard lock(mutex_); remote_msg_ = *msg; @@ -35,9 +36,9 @@ public: remote_valid_ = true; }); - sub_auto_ = this->create_subscription( + sub_auto_ = this->create_subscription( "/auto_mc_ctrl", 10, - [this](const mc::msg::McCtrl::SharedPtr msg) + [this](const sweeperMsg::McCtrl::SharedPtr msg) { std::lock_guard lock(mutex_); auto_msg_ = *msg; @@ -73,32 +74,27 @@ private: } // 所有控制源均失联，发布安全默认指令 - mc::msg::McCtrl safe_msg; - safe_msg.mcu_enabled = true; + sweeperMsg::McCtrl safe_msg; safe_msg.brake = 1; safe_msg.gear = 0; safe_msg.rpm = 0; - safe_msg.brake_time_ms = 500; safe_msg.angle = 0; safe_msg.angle_speed = 120; - safe_msg.main_brush_lift = false; safe_msg.edge_brush_lift = false; - safe_msg.vacuum = false; + safe_msg.sweep_ctrl = false; safe_msg.spray = false; safe_msg.mud_flap = false; safe_msg.dust_shake = false; - safe_msg.main_brush_spin = false; - safe_msg.edge_brush_spin = false; publisher_->publish(safe_msg); } - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Subscription::SharedPtr sub_radio_, sub_remote_, sub_auto_; + rclcpp::Subscription::SharedPtr sub_radio_, sub_remote_, sub_auto_; rclcpp::TimerBase::SharedPtr timer_; - mc::msg::McCtrl radio_msg_, remote_msg_, auto_msg_; + sweeperMsg::McCtrl radio_msg_, remote_msg_, auto_msg_; rclcpp::Time radio_last_time_, remote_last_time_, auto_last_time_; bool radio_valid_ = false; bool remote_valid_ = false; diff --git a/src/mc/CMakeLists.txt b/src/mc/CMakeLists.txt index 74e54a2..ed4d78f 100644 --- a/src/mc/CMakeLists.txt +++ b/src/mc/CMakeLists.txt @@ -6,16 +6,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # Find dependencies +find_package(sweeper_interfaces REQUIRED) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -# Generate message -rosidl_generate_interfaces(${PROJECT_NAME} "msg/CanFrame.msg" "msg/McCtrl.msg" - DEPENDENCIES std_msgs) - -ament_export_dependencies(rosidl_default_runtime) # 搜索 src 目录下所有 .cpp 文件 file(GLOB SRC_FILES src/*.cpp) @@ -23,33 +17,23 @@ file(GLOB SRC_FILES src/*.cpp) # 创建可执行文件 add_executable(mc_node ${SRC_FILES}) -ament_target_dependencies(mc_node rclcpp std_msgs) -rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} - "rosidl_typesupport_cpp") - -target_link_libraries(mc_node ${cpp_typesupport_target}) +ament_target_dependencies(mc_node rclcpp std_msgs sweeper_interfaces) # Set include directories for the target target_include_directories( mc_node PUBLIC $ - $ - $ $) -target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) # Require C99 and - # C++17 +target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) # Install target install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME}) - -# 安装配置目录 install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) # Handle testing if needed if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # Skip linter checking for copyrights if not relevant set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() diff --git a/src/mc/include/mc/can_driver.h b/src/mc/include/mc/can_driver.h index bb24355..db7af0e 100644 --- a/src/mc/include/mc/can_driver.h +++ b/src/mc/include/mc/can_driver.h @@ -6,18 +6,12 @@ #include #include #include - -#ifdef _WIN32 -#include -// 这里需要包含Windows平台特定的CAN头文件，例如PEAKCAN等 -#else #include #include #include #include #include #include -#endif struct CANFrame { @@ -60,13 +54,8 @@ public: private: void receiveThreadFunc(); bool applyFilters(); // 应用当前filters_ -#ifdef _WIN32 - // Windows平台相关成员 - HANDLE hCAN = INVALID_HANDLE_VALUE; -#else - int sockfd = -1; -#endif + int sockfd = -1; std::atomic running{false}; std::thread receiveThread; ReceiveCallback callback; diff --git a/src/mc/include/mc/can_struct.h b/src/mc/include/mc/can_struct.h index 56399ea..9053f3e 100644 --- a/src/mc/include/mc/can_struct.h +++ b/src/mc/include/mc/can_struct.h @@ -12,27 +12,27 @@ union can_MCU_cmd_union { struct { - // Byte 0 - uint8_t brake : 2; - uint8_t gear : 2; - uint8_t reserve1 : 1; - uint8_t mode : 2; - uint8_t enabled : 1; - - // Byte 1-2 + // Byte 0-1 + uint8_t reserve1; uint8_t reserve2; + + // Byte 2 + uint8_t rpm; + + // Byte 3 + uint8_t gear; + + // Byte 4 + uint8_t work_enabled; + + // Byte 5 + uint8_t brake; + + // Byte 6 uint8_t reserve3; - // Byte 3-4 - uint8_t rpm_h; - uint8_t rpm_l; - - // Byte 5-6 - uint8_t brakeTime_h; - uint8_t brakeTime_l; - // Byte 7 - uint8_t checksum; + uint8_t ctrl_enabled; } fields; uint8_t bytes[8]; @@ -61,15 +61,15 @@ struct can_MCU_cmd { can_MCU_cmd_union data; - static constexpr uint32_t CMD_ID = 0x1A1; - static constexpr bool EXT_FLAG = false; + static constexpr uint32_t CMD_ID = 0x0CFF17EF; + static constexpr bool EXT_FLAG = true; static constexpr bool RTR_FLAG = false; // 构造函数自动初始化常量 can_MCU_cmd() { memset(&data, 0, sizeof(data)); - data.fields.mode = 1; + data.fields.ctrl_enabled = 1; data.fields.reserve1 = 0; data.fields.reserve2 = 0; data.fields.reserve3 = 0; @@ -77,40 +77,22 @@ struct can_MCU_cmd void setEnabled(bool en) { - data.fields.enabled = en ? 1 : 0; + data.fields.work_enabled = en ? 1 : 0; } void setGear(uint8_t gear) { - data.fields.gear = gear & 0x03; + data.fields.gear = gear & 0x01; } void setBrake(uint8_t brake) { - data.fields.brake = brake & 0x03; + data.fields.brake = brake & 0x01; } - void setRPM(uint16_t rpm) + void setRPM(uint8_t rpm) { - data.fields.rpm_h = (rpm >> 8) & 0xFF; - data.fields.rpm_l = rpm & 0xFF; - } - - void setBrakeTime(uint16_t time_ms) - { - data.fields.brakeTime_h = (time_ms >> 8) & 0xFF; - data.fields.brakeTime_l = time_ms & 0xFF; - } - - void pack() - { - // 计算校验：Byte0 ~ Byte6 - uint8_t sum = 0; - for (int i = 0; i < 7; ++i) - { - sum += data.bytes[i]; - } - data.fields.checksum = ~sum; + data.fields.rpm = std::clamp(rpm, static_cast(0), static_cast(100)); } CANFrame toFrame() const @@ -136,16 +118,13 @@ struct can_EPS_cmd can_EPS_cmd() { memset(&data, 0, sizeof(data)); + data.fields.control_mode = 0x20; } - void setControlMode(uint8_t mode) + void setAngle(float angle) { - data.fields.control_mode = mode; - } - - void setAngle(int16_t angle) - { - int16_t raw = angle + 1024; // angle公式：receive[3]*256+receive[4]-1024 + // angle公式：receive[3]*256+receive[4]-1024 + int16_t raw = static_cast(angle * 7.0f + 1024.0f); data.fields.angle_h = (raw >> 8) & 0xFF; data.fields.angle_l = raw & 0xFF; } @@ -162,7 +141,7 @@ struct can_EPS_cmd value = 20; else if (value > 250) value = 250; - data.fields.angular_speed = value; + data.fields.angular_speed = static_cast(value); } void pack() @@ -194,22 +173,36 @@ struct can_VCU_out1_cmd static constexpr bool EXT_FLAG = true; static constexpr bool RTR_FLAG = false; + union Byte0 + { + struct + { + uint8_t reserved1 : 3; // 保留位 + uint8_t dust_shake : 1; // 振尘 + uint8_t reserved2 : 4; // 保留位 + } bits; + uint8_t raw = 0; + }; + union Byte3 { struct { - uint8_t night_light : 4; // 低 4 位（夜行灯等级） - uint8_t reserved : 3; // 保留位 - uint8_t leftLight : 1; // 左转灯 + uint8_t headlight : 4; // 大灯 + uint8_t reserved : 3; // 保留位 + uint8_t leftLight : 1; // 左转灯 } bits; uint8_t raw = 0; }; uint8_t data[8]{}; - Byte3 &byte3() + Byte0 &byte0() { return *reinterpret_cast(&data[0]); } + Byte3 &byte3() { return *reinterpret_cast(&data[3]); } + + void setDustShake(bool on) { - return *reinterpret_cast(&data[3]); + byte0().bits.dust_shake = on ? 1 : 0; } void setLeftLight(bool on) @@ -217,19 +210,9 @@ struct can_VCU_out1_cmd byte3().bits.leftLight = on ? 1 : 0; } - // 传入百分比（0~100），按每10%一级计算 - void setNightLightPercent(uint8_t percent) + void setHeadLight(bool on) { - if (percent > 100) - percent = 100; - uint8_t level = percent / 10; // 0–10 - byte3().bits.night_light = level; - } - - // 传入布尔值，true = 100%，false = 0% - void setNightLightPercent(bool on) - { - byte3().bits.night_light = on ? 0x0A : 0x00; + byte3().bits.headlight = on ? 0x64 : 0x00; } CANFrame toFrame() const @@ -265,10 +248,10 @@ struct can_VCU_out2_cmd { struct { - uint8_t reserved1 : 5; // 保留位 - uint8_t main_brush_lift : 1; // 主刷推杆 - uint8_t reserved2 : 1; // 保留位 - uint8_t headlight : 1; // 大灯 + uint8_t reserved1 : 5; // 保留位 + uint8_t sweep_ctrl : 1; // 扫地控制 + uint8_t reserved2 : 1; // 保留位 + uint8_t headlight : 1; // 大灯 } bits; uint8_t raw = 0; }; @@ -280,9 +263,7 @@ struct can_VCU_out2_cmd uint8_t edge_brush_lift : 1; // 边刷推杆 uint8_t reserved3 : 1; // 保留位 uint8_t spray : 1; // 喷水 - uint8_t reserved4 : 1; // 保留位 - uint8_t dust_shake : 1; // 振尘 - uint8_t vacuum : 1; // 吸尘 + uint8_t reserved4 : 3; // 保留位 uint8_t mud_flap : 1; // 挡皮 uint8_t reserved5 : 1; // 保留位 } bits; @@ -307,9 +288,9 @@ struct can_VCU_out2_cmd } // === Byte1 控制 === - void setMainBrushLift(bool on) + void setSweeepCtrl(bool on) { - byte1().bits.main_brush_lift = on ? 1 : 0; + byte1().bits.sweep_ctrl = on ? 1 : 0; } void setHeadlight(bool on) @@ -328,16 +309,6 @@ struct can_VCU_out2_cmd byte3().bits.spray = on ? 1 : 0; } - void setDustShake(bool on) - { - byte3().bits.dust_shake = on ? 1 : 0; - } - - void setVacuum(bool on) - { - byte3().bits.vacuum = on ? 1 : 0; - } - void setMudFlap(bool on) { byte3().bits.mud_flap = on ? 1 : 0; @@ -355,157 +326,9 @@ struct can_VCU_out2_cmd } }; -struct can_main_Brush_cmd -{ - static constexpr uint32_t CMD_ID = 0x0cee36ef; - static constexpr bool EXT_FLAG = true; - static constexpr bool RTR_FLAG = false; - - union Byte0 - { - struct - { - uint8_t right_motor_pwm : 8; - } bits; - uint8_t raw = 0; - }; - - union Byte1 - { - struct - { - uint8_t left_motor_pwm : 8; - } bits; - uint8_t raw = 0; - }; - - union Byte7 - { - struct - { - uint8_t can_enabled : 8; - } bits; - uint8_t raw = 0; - }; - - uint8_t data[8]{}; - - can_main_Brush_cmd() - { - data[2] = 1; // 固定字节初始化 - } - - Byte0 &byte0() { return *reinterpret_cast(&data[0]); } - Byte1 &byte1() { return *reinterpret_cast(&data[1]); } - Byte7 &byte7() { return *reinterpret_cast(&data[7]); } - - // === Byte0,1 控制 === - void setBrushPwm(uint8_t pwm) - { - if (pwm > 100) - { - pwm = 100; - } - byte0().bits.right_motor_pwm = pwm; - byte1().bits.left_motor_pwm = pwm; - } - - // === Byte7 控制 === - void setBrushEnable(bool on) - { - byte7().bits.can_enabled = on ? 1 : 0; - } - - CANFrame toFrame() const - { - CANFrame frame; - frame.id = CMD_ID; - std::copy(std::begin(data), std::end(data), frame.data); - frame.dlc = 8; - frame.ext = EXT_FLAG; - frame.rtr = RTR_FLAG; - return frame; - } -}; - -struct can_edge_Brush_cmd -{ - static constexpr uint32_t CMD_ID = 0x0cee26ef; - static constexpr bool EXT_FLAG = true; - static constexpr bool RTR_FLAG = false; - - union Byte0 - { - struct - { - uint8_t right_motor_pwm : 8; - } bits; - uint8_t raw = 0; - }; - - union Byte1 - { - struct - { - uint8_t left_motor_pwm : 8; - } bits; - uint8_t raw = 0; - }; - - union Byte7 - { - struct - { - uint8_t can_enabled : 8; - } bits; - uint8_t raw = 0; - }; - - uint8_t data[8]{}; - - can_edge_Brush_cmd() - { - data[2] = 1; // 固定字节初始化 - } - - Byte0 &byte0() { return *reinterpret_cast(&data[0]); } - Byte1 &byte1() { return *reinterpret_cast(&data[1]); } - Byte7 &byte7() { return *reinterpret_cast(&data[7]); } - - // === Byte0,1 控制 === - void setBrushPwm(uint8_t pwm) - { - if (pwm > 100) - { - pwm = 100; - } - byte0().bits.right_motor_pwm = pwm; - byte1().bits.left_motor_pwm = pwm; - } - - // === Byte7 控制 === - void setBrushEnable(bool on) - { - byte7().bits.can_enabled = on ? 1 : 0; - } - - CANFrame toFrame() const - { - CANFrame frame; - frame.id = CMD_ID; - std::copy(std::begin(data), std::end(data), frame.data); - frame.dlc = 8; - frame.ext = EXT_FLAG; - frame.rtr = RTR_FLAG; - return frame; - } -}; - extern can_MCU_cmd mcu_cmd; extern can_EPS_cmd eps_cmd; extern can_VCU_out1_cmd vcu1_cmd; extern can_VCU_out2_cmd vcu2_cmd; -extern can_main_Brush_cmd main_brush_cmd; -extern can_edge_Brush_cmd edge_brush_cmd; #endif diff --git a/src/mc/msg/McCtrl.msg b/src/mc/msg/McCtrl.msg deleted file mode 100644 index 0c7e24a..0000000 --- a/src/mc/msg/McCtrl.msg +++ /dev/null @@ -1,31 +0,0 @@ -#清扫车控制指令 - -#mcu部分 -bool mcu_enabled #是否使能 -uint8 brake #电磁刹指令 0 放开 1 抱死 -uint8 gear #挡位 0x00:void 0x01:R档 0x02:N档 0x03:D档 -uint16 rpm #目标转速 -uint16 brake_time_ms #刹车时间 500-30000ms - -#eps部分 -float32 angle #轮端转向角度 分辨率0.2° [-60.0,60.0] 适当缩减 -uint16 angle_speed #转向角速度 120-1500rpm - -#vcu部分 -bool main_brush_lift #主刷推杆 -bool edge_brush_lift #边刷推杆 -bool vacuum #吸尘 -bool spray #喷水 -bool mud_flap #挡皮 -bool dust_shake #振尘 - -bool left_light #左转向灯 -bool right_light #右转向灯 -bool night_light #夜行灯 -bool brake_light #刹车灯 -bool headlight #大灯 - -bool main_brush_spin #主刷旋转 -bool edge_brush_spin #边刷旋转 -uint8 main_brush_pwm #主刷转速 -uint8 edge_brush_pwm #主刷转速 \ No newline at end of file diff --git a/src/mc/package.xml b/src/mc/package.xml index 2911d56..3e506a3 100644 --- a/src/mc/package.xml +++ b/src/mc/package.xml @@ -8,17 +8,9 @@ ament_cmake - rosidl_default_generators + sweeper_interfaces + rclcpp rosidl_default_runtime - builtin_interfaces - builtin_interfaces - rosidl_interface_packages - - rclcpp - std_msgs - - rclcpp - std_msgs ament_cmake diff --git a/src/mc/src/can_driver.cpp b/src/mc/src/can_driver.cpp index 7204296..3af1f9f 100644 --- a/src/mc/src/can_driver.cpp +++ b/src/mc/src/can_driver.cpp @@ -3,8 +3,12 @@ #include #include #include -#include // 用于 fcntl -#include // 用于 close 和 read +#include +#include +#include +#include +#include +#include CANDriver::CANDriver() = default; @@ -16,23 +20,19 @@ CANDriver::~CANDriver() bool CANDriver::open(const std::string &interface) { if (running) - { return false; - } -#ifdef _WIN32 - std::cerr << "Windows implementation not available\n"; - return false; -#else - // Linux SocketCAN实现 - if ((sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) + sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (sockfd < 0) { perror("socket"); return false; } struct ifreq ifr; - strcpy(ifr.ifr_name, interface.c_str()); + std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0) { perror("ioctl"); @@ -40,50 +40,39 @@ bool CANDriver::open(const std::string &interface) return false; } - struct sockaddr_can addr; + struct sockaddr_can addr{}; addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; - if (bind(sockfd, (struct sockaddr *)&addr, sizeof(addr)) < 0) + if (bind(sockfd, reinterpret_cast(&addr), sizeof(addr)) < 0) { perror("bind"); ::close(sockfd); return false; } - // 设置非阻塞模式 + // 设置为非阻塞 int flags = fcntl(sockfd, F_GETFL, 0); - fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); // 设置为非阻塞 + fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); running = true; receiveThread = std::thread(&CANDriver::receiveThreadFunc, this); return true; -#endif } void CANDriver::close() { - if (running) - { - running = false; - if (receiveThread.joinable()) - { - receiveThread.join(); - } + if (!running) + return; -#ifdef _WIN32 - if (hCAN != INVALID_HANDLE_VALUE) - { - // CAN_Close(hCAN); - hCAN = INVALID_HANDLE_VALUE; - } -#else - if (sockfd >= 0) - { - ::close(sockfd); - sockfd = -1; - } -#endif + running = false; + if (receiveThread.joinable()) + receiveThread.join(); + + if (sockfd >= 0) + { + ::close(sockfd); + sockfd = -1; } } @@ -92,32 +81,22 @@ bool CANDriver::sendFrame(const CANFrame &frame) if (!running) return false; -#ifdef _WIN32 - // Windows发送实现 - return false; -#else - struct can_frame raw_frame; - memset(&raw_frame, 0, sizeof(raw_frame)); - + struct can_frame raw_frame{}; raw_frame.can_id = frame.id; if (frame.ext) raw_frame.can_id |= CAN_EFF_FLAG; if (frame.rtr) raw_frame.can_id |= CAN_RTR_FLAG; raw_frame.can_dlc = frame.dlc; - memcpy(raw_frame.data, frame.data, frame.dlc); - - // 设置非阻塞写操作 - int flags = fcntl(sockfd, F_GETFL, 0); - fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); // 设置为非阻塞 + std::memcpy(raw_frame.data, frame.data, frame.dlc); if (write(sockfd, &raw_frame, sizeof(raw_frame)) != sizeof(raw_frame)) { perror("write"); return false; } + return true; -#endif } void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData) @@ -128,15 +107,11 @@ void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData) bool CANDriver::setFilter(const std::vector &filters) { -#ifdef _WIN32 - return false; -#else if (!running) return false; - filters_ = filters; // 更新本地保存 + filters_ = filters; return applyFilters(); -#endif } bool CANDriver::addFilter(const can_filter &filter) @@ -153,15 +128,11 @@ bool CANDriver::addFilters(const std::vector &filters) bool CANDriver::applyFilters() { -#ifdef _WIN32 - return false; -#else if (!running) return false; if (filters_.empty()) { - // 如果没有设置过滤器，就关闭硬件过滤器（接受所有帧） setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, nullptr, 0); return true; } @@ -172,41 +143,32 @@ bool CANDriver::applyFilters() perror("setsockopt"); return false; } + return true; -#endif } void CANDriver::receiveThreadFunc() { -#ifdef _WIN32 - // Windows接收实现 -#else struct can_frame raw_frame; while (running) { - // 在非阻塞模式下读取数据 ssize_t nbytes = read(sockfd, &raw_frame, sizeof(raw_frame)); if (nbytes < 0) { - if (errno != EAGAIN) // 非阻塞错误，忽略 - { + if (errno != EAGAIN) perror("read"); - } - continue; // 没有数据时继续循环 + continue; } if (nbytes == sizeof(raw_frame) && callback) { CANFrame frame; frame.id = raw_frame.can_id & CAN_EFF_MASK; - frame.ext = !!(raw_frame.can_id & CAN_EFF_FLAG); - frame.rtr = !!(raw_frame.can_id & CAN_RTR_FLAG); + frame.ext = static_cast(raw_frame.can_id & CAN_EFF_FLAG); + frame.rtr = static_cast(raw_frame.can_id & CAN_RTR_FLAG); frame.dlc = raw_frame.can_dlc; - memcpy(frame.data, raw_frame.data, raw_frame.can_dlc); - - // 调用回调函数，处理接收到的帧 + std::memcpy(frame.data, raw_frame.data, raw_frame.can_dlc); callback(frame, userData); } } -#endif } diff --git a/src/mc/src/can_struct.cpp b/src/mc/src/can_struct.cpp index fdf6379..9e2659b 100644 --- a/src/mc/src/can_struct.cpp +++ b/src/mc/src/can_struct.cpp @@ -4,5 +4,3 @@ can_MCU_cmd mcu_cmd; can_EPS_cmd eps_cmd; can_VCU_out1_cmd vcu1_cmd; can_VCU_out2_cmd vcu2_cmd; -can_main_Brush_cmd main_brush_cmd; -can_edge_Brush_cmd edge_brush_cmd; \ No newline at end of file diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 6225885..f8ca8ef 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -5,21 +5,22 @@ #include "mc/can_struct.h" #include "mc/get_config.h" #include "rclcpp/rclcpp.hpp" -#include "mc/msg/can_frame.hpp" -#include "mc/msg/mc_ctrl.hpp" +#include "sweeper_interfaces/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" + +namespace sweeperMsg = sweeper_interfaces::msg; CANDriver canctl; struct CanHandlerContext { rclcpp::Node::SharedPtr node; - std::shared_ptr> publisher; + std::shared_ptr> publisher; }; -std::atomic estop_active = false; // 急停状态量 -std::atomic estop_msg_received = false; // 是否收到过急停帧 -std::atomic vcu_awake = false; // 是否唤醒成功 -rclcpp::Time last_estop_msg_time; // 上次收到急停帧的时间 +std::atomic vcu_msg_received = false; // 是否收到过vcu数据帧 +std::atomic vcu_awake = false; // 是否唤醒成功 +rclcpp::Time last_vcu_msg_time; // 上次收到vcu数据帧的时间 // 全局开关，控制是否打印 CAN 消息到终端 bool g_can_print_enable = false; @@ -27,11 +28,11 @@ bool g_can_print_enable = false; struct ControlCache { std::mutex mutex; // 防止多线程冲突 - mc::msg::McCtrl latest_msg; + sweeperMsg::McCtrl latest_msg; std::chrono::steady_clock::time_point last_update_time; bool has_data = false; - void update(const mc::msg::McCtrl &msg) + void update(const sweeperMsg::McCtrl &msg) { std::lock_guard lock(mutex); latest_msg = msg; @@ -39,7 +40,7 @@ struct ControlCache has_data = true; } - bool get(mc::msg::McCtrl &msg) + bool get(sweeperMsg::McCtrl &msg) { std::lock_guard lock(mutex); if (!has_data) @@ -61,27 +62,22 @@ struct ControlCache ControlCache control_cache; // 发布检测 -mc::msg::McCtrl get_safe_control() +sweeperMsg::McCtrl get_safe_control() { - mc::msg::McCtrl msg; + sweeperMsg::McCtrl msg; if (!control_cache.get(msg)) { // 超时或未接收到控制数据，进入安全状态 - msg.mcu_enabled = true; msg.brake = 1; msg.gear = 0; msg.rpm = 0; - msg.brake_time_ms = 500; msg.angle = 0; msg.angle_speed = 120; - msg.main_brush_lift = false; msg.edge_brush_lift = false; - msg.vacuum = false; + msg.sweep_ctrl = false; msg.spray = false; msg.mud_flap = false; msg.dust_shake = false; - msg.main_brush_spin = false; - msg.edge_brush_spin = false; } return msg; } @@ -96,7 +92,7 @@ void receiveHandler(const CANFrame &frame, void *userData) auto now = node->now(); // 创建并发布 CAN 消息 - auto msg = mc::msg::CanFrame(); + auto msg = sweeperMsg::CanFrame(); msg.id = frame.id; msg.dlc = frame.dlc; msg.data.assign(frame.data, frame.data + frame.dlc); @@ -121,61 +117,17 @@ void receiveHandler(const CANFrame &frame, void *userData) RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str()); } - // 急停报文检查 - if (frame.id == 0x18FA0121 && frame.dlc >= 3) + // VCU报文检查 + if (frame.id == 0x18FA0121) { - last_estop_msg_time = now; // 更新时间 - estop_msg_received.store(true); + last_vcu_msg_time = now; // 更新时间 + vcu_msg_received.store(true); vcu_awake.store(true); // 唤醒成功 - - uint8_t estop_status = (frame.data[2] & 0x0c) >> 2; // 急停状态位 - - // 急停被按下 - if (estop_status != 0x03) - { - if (!estop_active.load()) - { - RCLCPP_WARN(node->get_logger(), "E-STOP TRIGGERED, entering safe mode"); - } - - estop_active.store(true); - - // 立即更新控制缓存，强制进入安全状态 - mc::msg::McCtrl safe_msg; - safe_msg.mcu_enabled = true; - safe_msg.brake = 1; - safe_msg.gear = 0; - safe_msg.rpm = 0; - safe_msg.brake_time_ms = 500; - safe_msg.angle = 0; - safe_msg.angle_speed = 120; - safe_msg.main_brush_lift = false; - safe_msg.edge_brush_lift = false; - safe_msg.vacuum = false; - safe_msg.spray = false; - safe_msg.mud_flap = false; - safe_msg.dust_shake = false; - safe_msg.main_brush_spin = false; - safe_msg.edge_brush_spin = false; - - control_cache.update(safe_msg); // 用这个覆盖已有控制指令 - } - else - { - if (estop_active.load()) - { - RCLCPP_INFO(node->get_logger(), "E-STOP released, resuming control"); - } - estop_active.store(false); - } } } -void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg) +void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) { - if (estop_active) - return; - control_cache.update(*msg); } @@ -219,17 +171,17 @@ void setupTimers(rclcpp::Node::SharedPtr node) VCUWakeUp(); } }); - // 急停报文 watchdog 检查，200ms + // 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_estop_msg_time; + auto elapsed = now - last_vcu_msg_time; - if (estop_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) + if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) { - RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] Emergency stop message timeout, resetting wake-up state."); - estop_msg_received.store(false); + RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state."); + vcu_msg_received.store(false); vcu_awake.store(false); } }); @@ -237,14 +189,11 @@ void setupTimers(rclcpp::Node::SharedPtr node) static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer( std::chrono::milliseconds(20), []() { - mc::msg::McCtrl msg = get_safe_control(); + sweeperMsg::McCtrl msg = get_safe_control(); - mcu_cmd.setEnabled(msg.mcu_enabled); mcu_cmd.setGear(msg.gear); mcu_cmd.setRPM(msg.rpm); - mcu_cmd.setBrakeTime(msg.brake_time_ms); mcu_cmd.setBrake(msg.brake); - mcu_cmd.pack(); canctl.sendFrame(mcu_cmd.toFrame()); }); @@ -252,52 +201,35 @@ void setupTimers(rclcpp::Node::SharedPtr node) static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer( std::chrono::milliseconds(50), []() { - mc::msg::McCtrl msg = get_safe_control(); + sweeperMsg::McCtrl msg = get_safe_control(); eps_cmd.setCenterCmd(0); - eps_cmd.setAngle(static_cast(msg.angle * 5)); + eps_cmd.setAngle(msg.angle); eps_cmd.setAngularSpeed(msg.angle_speed); - eps_cmd.setControlMode(0x20); eps_cmd.pack(); canctl.sendFrame(eps_cmd.toFrame()); }); - // VCU2 控制，10Hz + // VCU 控制，10Hz static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer( std::chrono::milliseconds(100), []() { - mc::msg::McCtrl msg = get_safe_control(); + sweeperMsg::McCtrl msg = get_safe_control(); vcu1_cmd.setLeftLight(msg.left_light); - vcu1_cmd.setNightLightPercent(msg.night_light); + vcu1_cmd.setDustShake(msg.dust_shake); + vcu1_cmd.setHeadLight(msg.headlight); vcu2_cmd.setBrakeLight(msg.brake_light); - vcu2_cmd.setDustShake(msg.dust_shake); vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift); vcu2_cmd.setHeadlight(msg.headlight); - vcu2_cmd.setMainBrushLift(msg.main_brush_lift); vcu2_cmd.setMudFlap(msg.mud_flap); vcu2_cmd.setRightLight(msg.right_light); vcu2_cmd.setSpray(msg.spray); - vcu2_cmd.setVacuum(msg.vacuum); + vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl); canctl.sendFrame(vcu1_cmd.toFrame()); canctl.sendFrame(vcu2_cmd.toFrame()); }); - - // 刷子控制，3.3Hz - static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer( - std::chrono::milliseconds(300), []() - { - mc::msg::McCtrl msg = get_safe_control(); - - main_brush_cmd.setBrushEnable(msg.main_brush_spin); - main_brush_cmd.setBrushPwm(msg.main_brush_pwm); - - edge_brush_cmd.setBrushEnable(msg.edge_brush_spin); - edge_brush_cmd.setBrushPwm(msg.edge_brush_pwm); - - canctl.sendFrame(main_brush_cmd.toFrame()); - canctl.sendFrame(edge_brush_cmd.toFrame()); }); } int main(int argc, char **argv) @@ -310,16 +242,16 @@ int main(int argc, char **argv) RCLCPP_INFO(node->get_logger(), "Starting mc package..."); // 创建一个 Publisher - auto can_publisher = node->create_publisher("can_data", 10); + auto can_publisher = node->create_publisher("can_data", 10); // 创建 Subscriber（收控制指令） - auto subscriber = node->create_subscription( + auto subscriber = node->create_subscription( "mc_ctrl", // 订阅的 topic 名字 10, // 队列长度 mcCtrlCallback // 收到消息后的回调 ); - last_estop_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type()); + last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type()); Config mc_config; load_config(mc_config); diff --git a/src/mqtt_report/CMakeLists.txt b/src/mqtt_report/CMakeLists.txt index e2be74b..ee74b32 100644 --- a/src/mqtt_report/CMakeLists.txt +++ b/src/mqtt_report/CMakeLists.txt @@ -13,7 +13,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(mc REQUIRED) +find_package(sweeper_interfaces REQUIRED) find_package(radio_ctrl REQUIRED) find_package(std_msgs REQUIRED) @@ -21,7 +21,7 @@ file(GLOB MQTT_REPORT_SOURCES "src/*.cpp") add_executable(mqtt_report_node ${MQTT_REPORT_SOURCES}) -ament_target_dependencies(mqtt_report_node rclcpp mc std_msgs) +ament_target_dependencies(mqtt_report_node rclcpp sweeper_interfaces std_msgs) # 虚拟头文件库，包含项目头文件路径 add_library(mqtt_headers INTERFACE) diff --git a/src/mqtt_report/package.xml b/src/mqtt_report/package.xml index a53c07f..06e7468 100644 --- a/src/mqtt_report/package.xml +++ b/src/mqtt_report/package.xml @@ -8,7 +8,7 @@ MIT rclcpp - mc + sweeper_interfaces radio_ctrl ament_cmake diff --git a/src/mqtt_report/src/mqtt_report.cpp b/src/mqtt_report/src/mqtt_report.cpp index 765fafd..d1bbd6a 100644 --- a/src/mqtt_report/src/mqtt_report.cpp +++ b/src/mqtt_report/src/mqtt_report.cpp @@ -1,11 +1,13 @@ #include -#include "mc/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/can_frame.hpp" #include "mqtt_report/mqtt_client.hpp" #include "mqtt_report/get_config.h" #include "mqtt_report/report_struct.h" #include "mqtt_report/fault_codes.h" #include +namespace sweeperMsg = sweeper_interfaces::msg; + Config config; // 清扫车配置文件 GeneralMsg info_report; // 常规消息上报 @@ -31,7 +33,7 @@ void updateWaterLevel(float newLevel) } // 解析can报文，做消息上报 -void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg) +void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) { switch (msg->id) { @@ -230,7 +232,7 @@ public: RCLCPP_ERROR(this->get_logger(), "Failed to connect to MQTT server"); } - subscription_ = this->create_subscription( + subscription_ = this->create_subscription( "can_data", 10, std::bind(&CanDataSubscriber::topic_callback, this, std::placeholders::_1)); info_timer_ = this->create_wall_timer( @@ -243,7 +245,7 @@ public: } private: - void topic_callback(const mc::msg::CanFrame::SharedPtr msg) + void topic_callback(const sweeperMsg::CanFrame::SharedPtr msg) { // RCLCPP_INFO(this->get_logger(), "Received CAN frame ID: %u, DLC: %u", msg->id, msg->dlc); Msg_Handler(msg); @@ -265,7 +267,7 @@ private: } } - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_; MQTTClientWrapper mqtt_client_; std::string info_topic_; std::string fault_topic_; diff --git a/src/radio_ctrl/CMakeLists.txt b/src/radio_ctrl/CMakeLists.txt index e6c0691..56f8808 100644 --- a/src/radio_ctrl/CMakeLists.txt +++ b/src/radio_ctrl/CMakeLists.txt @@ -8,17 +8,16 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(mc REQUIRED) -find_package(std_msgs REQUIRED) # 若使用自定义消息，这里替换为对应 msg 包 +find_package(sweeper_interfaces REQUIRED) +find_package(std_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) -# find_package(sensor_msgs REQUIRED) # 若用 Joy 消息就加这个 add_executable(radio_ctrl_node src/radio_ctrl.cpp src/uart_handler.cpp) target_include_directories( radio_ctrl_node PUBLIC $) -ament_target_dependencies(radio_ctrl_node rclcpp std_msgs mc) +ament_target_dependencies(radio_ctrl_node rclcpp std_msgs sweeper_interfaces) install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME}) diff --git a/src/radio_ctrl/config/config.json b/src/radio_ctrl/config/config.json index cbb545a..9c3d9d2 100644 --- a/src/radio_ctrl/config/config.json +++ b/src/radio_ctrl/config/config.json @@ -2,7 +2,7 @@ "serial_port": "/dev/ttyUSB0", "baudrate": 100000, "rmoctl_para": { - "mcu_rpm_max": 1000, - "eps_angle_max": 30.0 + "mcu_rpm_max": 100, + "eps_angle_max": 50.0 } -} +} \ No newline at end of file diff --git a/src/radio_ctrl/package.xml b/src/radio_ctrl/package.xml index b9b3127..86df599 100644 --- a/src/radio_ctrl/package.xml +++ b/src/radio_ctrl/package.xml @@ -11,9 +11,7 @@ rclcpp std_msgs - mc - - + sweeper_interfaces ament_cmake ament_cmake diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index fdd7fe4..41124ac 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -3,8 +3,10 @@ #include #include #include -#include "mc/msg/mc_ctrl.hpp" -#include "mc/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" +#include "sweeper_interfaces/msg/can_frame.hpp" + +namespace sweeperMsg = sweeper_interfaces::msg; constexpr float EPS_GEAR_RATIO = 16.5f; constexpr float DELTA_T = 0.02f; // 20ms @@ -24,10 +26,10 @@ public: } // 发布控制指令消息的发布器 - pub_ = this->create_publisher("radio_mc_ctrl", 10); + pub_ = this->create_publisher("radio_mc_ctrl", 10); // 订阅CAN反馈的回调函数 - can_sub_ = this->create_subscription( + can_sub_ = this->create_subscription( "can_data", 10, std::bind(&SBUSNode::can_callback, this, std::placeholders::_1)); @@ -63,8 +65,8 @@ private: bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 - auto msg = mc::msg::McCtrl(); // 控制消息对象 - uint16_t ch_data[8]; // 各通道遥控数据 + auto msg = sweeperMsg::McCtrl(); // 控制消息对象 + uint16_t ch_data[8]; // 各通道遥控数据 if (data_safe) // 数据安全，进行数据解析并发布 { @@ -78,64 +80,45 @@ private: if (ch_data[6] == 192) // 是否使能车辆控制 { - msg.mcu_enabled = true; msg.brake = 0; // 挡位选择 if (ch_data[7] == 192) { - msg.gear = 3; // D挡 + msg.gear = 0; // D挡 } else if (ch_data[7] == 1792) { msg.gear = 1; // R挡 } - else if (ch_data[7] == 992) - { - msg.gear = 2; // N挡 - } - else - { - msg.gear = 0; // 未知状态 - } // 油门 / 刹车逻辑 if (ch_data[1] <= speed[1]) { msg.brake = 1; - msg.brake_time_ms = static_cast(30000 + (500 - 30000) * (speed[1] - ch_data[1]) / (speed[1] - speed[0])); msg.rpm = 0; } else { msg.brake = 0; - msg.brake_time_ms = 500; msg.rpm = static_cast(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1])); } // 一键清扫 if (ch_data[5] == 1792) { - msg.main_brush_lift = false; msg.edge_brush_lift = false; - msg.vacuum = false; + msg.sweep_ctrl = false; msg.spray = false; msg.mud_flap = false; msg.dust_shake = false; - msg.main_brush_spin = false; - msg.edge_brush_spin = false; } else if (ch_data[5] == 192) { - msg.main_brush_lift = true; msg.edge_brush_lift = true; - msg.vacuum = true; + msg.sweep_ctrl = true; msg.spray = true; msg.mud_flap = true; msg.dust_shake = false; - msg.main_brush_spin = true; - msg.edge_brush_spin = true; - msg.main_brush_pwm = 100; - msg.edge_brush_pwm = 100; } // 转向逻辑 @@ -158,49 +141,38 @@ private: } else // 未使能状态，发送安全默认控制指令 { - msg.mcu_enabled = true; msg.brake = 1; msg.gear = 0; msg.rpm = 0; - msg.brake_time_ms = 500; msg.angle = 0; msg.angle_speed = 120; - msg.main_brush_lift = false; msg.edge_brush_lift = false; - msg.vacuum = false; + msg.sweep_ctrl = false; msg.spray = false; msg.mud_flap = false; msg.dust_shake = false; - msg.main_brush_spin = false; - msg.edge_brush_spin = false; } // 发布控制消息 pub_->publish(msg); RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - << "\n mcu_enabled: " << msg.mcu_enabled << "\n brake: " << static_cast(msg.brake) << "\n gear: " << static_cast(msg.gear) << "\n rpm: " << msg.rpm - << "\n brake_time_ms: " << msg.brake_time_ms + << "\n rpm: " << msg.ehb_anable + << "\n angle: " << msg.ehb_brake_pressure << "\n angle: " << msg.angle << "\n angle_speed: " << msg.angle_speed - << "\n main_brush_lift: " << msg.main_brush_lift << "\n edge_brush_lift: " << msg.edge_brush_lift - << "\n vacuum: " << msg.vacuum + << "\n vacuum: " << msg.sweep_ctrl << "\n spray: " << msg.spray << "\n mud_flap: " << msg.mud_flap << "\n dust_shake: " << msg.dust_shake << "\n left_light: " << msg.left_light << "\n right_light: " << msg.right_light - << "\n night_light: " << msg.night_light << "\n brake_light: " << msg.brake_light - << "\n headlight: " << msg.headlight - << "\n main_brush_spin: " << msg.main_brush_spin - << "\n edge_brush_spin: " << msg.edge_brush_spin - << "\n main_brush_pwm: " << static_cast(msg.main_brush_pwm) - << "\n edge_brush_pwm: " << static_cast(msg.edge_brush_pwm)); + << "\n headlight: " << msg.headlight); } else { @@ -210,7 +182,7 @@ private: } // CAN反馈回调函数（用于获取当前转向角度） - void can_callback(const mc::msg::CanFrame::SharedPtr msg) + void can_callback(const sweeperMsg::CanFrame::SharedPtr msg) { // 判断是否为转向反馈帧（ID=0x401，且数据长度大于5） if (msg->id == 0x401 && msg->dlc >= 5) @@ -224,8 +196,8 @@ private: } // ROS2 发布/订阅/定时器/串口读取器 - rclcpp::Publisher::SharedPtr pub_; - rclcpp::Subscription::SharedPtr can_sub_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr can_sub_; rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr uart_handler_; diff --git a/src/sweeper_interfaces/CMakeLists.txt b/src/sweeper_interfaces/CMakeLists.txt new file mode 100644 index 0000000..f62d869 --- /dev/null +++ b/src/sweeper_interfaces/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +project(sweeper_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/McCtrl.msg" + "msg/CanFrame.msg" + DEPENDENCIES std_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies(rosidl_default_runtime) +ament_export_dependencies(rosidl_default_generators) + +install( + DIRECTORY msg + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/mc/msg/CanFrame.msg b/src/sweeper_interfaces/msg/CanFrame.msg similarity index 100% rename from src/mc/msg/CanFrame.msg rename to src/sweeper_interfaces/msg/CanFrame.msg diff --git a/src/sweeper_interfaces/msg/McCtrl.msg b/src/sweeper_interfaces/msg/McCtrl.msg new file mode 100644 index 0000000..971a0f5 --- /dev/null +++ b/src/sweeper_interfaces/msg/McCtrl.msg @@ -0,0 +1,26 @@ +#清扫车控制指令 + +#mcu部分 +uint8 brake #电磁刹指令 0 放开 1 抱死 +uint8 gear #挡位 0x00:D档 0x01:R档 +uint8 rpm #转速占空比 0-100 + +#ehb部分 +bool ehb_anable #ehb使能 +uint8 ehb_brake_pressure #ehb制动压力 0-8MPa + +#eps部分 +float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减 +uint16 angle_speed #转向角速度 120-1500rpm + +#vcu部分 +bool edge_brush_lift #边刷推杆 +bool sweep_ctrl #扫地控制 +bool spray #喷水 +bool mud_flap #挡皮 +bool dust_shake #振尘 + +bool left_light #左转向灯 +bool right_light #右转向灯 +bool brake_light #刹车灯 +bool headlight #大灯 \ No newline at end of file diff --git a/src/sweeper_interfaces/package.xml b/src/sweeper_interfaces/package.xml new file mode 100644 index 0000000..c02b80f --- /dev/null +++ b/src/sweeper_interfaces/package.xml @@ -0,0 +1,23 @@ + + + + sweeper_interfaces + 0.0.0 + Custom message and service definitions for sweeper project. + root + MIT + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + +