From 65d9734dafcb456d8361b3644f20faf0244e4cd5 Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Wed, 6 May 2026 13:23:28 +0800 Subject: [PATCH] uss debug --- src/perception/uss/include/uss.h | 4 +-- src/perception/uss/src/uss.cpp | 9 +++--- src/perception/uss/src/uss_node.cpp | 46 +++++++++++------------------ 3 files changed, 24 insertions(+), 35 deletions(-) diff --git a/src/perception/uss/include/uss.h b/src/perception/uss/include/uss.h index 25773ec..3af392d 100644 --- a/src/perception/uss/include/uss.h +++ b/src/perception/uss/include/uss.h @@ -21,8 +21,8 @@ class UssDriver struct UssData { - uint8_t distances[8]; // 8路超声波距离数据 (cm) - uint8_t distances_calib[8]; // 8路校准后距离数据 (cm) + uint8_t distances[8]; // 8路超声波原始距离数据 (cm) + uint16_t distances_calib[8]; // 8路校准后距离数据 (cm) bool valid; }; diff --git a/src/perception/uss/src/uss.cpp b/src/perception/uss/src/uss.cpp index f73574b..df633e5 100644 --- a/src/perception/uss/src/uss.cpp +++ b/src/perception/uss/src/uss.cpp @@ -6,7 +6,6 @@ // Priority: 0x18, PGN: 0xEC00 (60416), Device ID: 0x57 (87) constexpr uint32_t USS_CAN_ID = 0x18EC0057; constexpr uint32_t USS_CAN_MASK = 0xFFFFFFFF; // 精确匹配 - UssDriver::UssDriver() : print_debug_(false), initialized_(false) { // 初始化校准偏移 @@ -42,7 +41,7 @@ bool UssDriver::init(const std::string& can_interface) // 设置CAN过滤器，只接收超声波的CAN数据 struct can_filter filter; filter.can_id = USS_CAN_ID | CAN_EFF_FLAG; - filter.can_mask = CAN_EFF_MASK | CAN_EFF_FLAG; + filter.can_mask = USS_CAN_MASK; std::vector filters; filters.push_back(filter); @@ -92,7 +91,7 @@ void UssDriver::setCalibration(int index, int offset) // 更新校准后的数据 std::lock_guard lock(data_mutex_); int calib_distance = static_cast(uss_data_.distances[index]) + offset; - uss_data_.distances_calib[index] = static_cast(std::max(0, std::min(255, calib_distance))); + uss_data_.distances_calib[index] = static_cast(std::max(0, calib_distance)); } } @@ -107,7 +106,7 @@ void UssDriver::setAllCalibrations(const int offsets[8]) for (int i = 0; i < 8; i++) { int calib_distance = static_cast(uss_data_.distances[i]) + calibration_offsets_[i]; - uss_data_.distances_calib[i] = static_cast(std::max(0, std::min(255, calib_distance))); + uss_data_.distances_calib[i] = static_cast(std::max(0, calib_distance)); } } @@ -135,7 +134,7 @@ void UssDriver::processCanFrame(const CANFrame& frame) uss_data_.distances[i] = frame.data[i]; // 计算校准后的数据 int calib_distance = static_cast(frame.data[i]) + calibration_offsets_[i]; - uss_data_.distances_calib[i] = static_cast(std::max(0, std::min(255, calib_distance))); + uss_data_.distances_calib[i] = static_cast(std::max(0, calib_distance)); } uss_data_.valid = true; diff --git a/src/perception/uss/src/uss_node.cpp b/src/perception/uss/src/uss_node.cpp index 5590c22..f7fcae7 100644 --- a/src/perception/uss/src/uss_node.cpp +++ b/src/perception/uss/src/uss_node.cpp @@ -2,7 +2,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/int8_multi_array.hpp" +#include "std_msgs/msg/u_int16_multi_array.hpp" #include "uss.h" class UssNode : public rclcpp::Node @@ -43,7 +43,7 @@ class UssNode : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Config loaded successfully"); // 创建发布者 - uss_publisher_ = this->create_publisher("USS", 10); + uss_publisher_ = this->create_publisher("USS", 10); // 创建定时器，定期发布数据 timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / publish_rate), @@ -57,7 +57,7 @@ class UssNode : public rclcpp::Node private: void timer_callback() { - auto message = std_msgs::msg::Int8MultiArray(); + auto message = std_msgs::msg::UInt16MultiArray(); const UssDriver::UssData& uss_data = uss_driver_.getUssData(); if (uss_data.valid) @@ -65,35 +65,25 @@ class UssNode : public rclcpp::Node message.data.clear(); message.data.resize(8); - message.data[0] = static_cast(uss_data.distances_calib[0]); - message.data[1] = static_cast(uss_data.distances_calib[1]); - - message.data[2] = static_cast(uss_data.distances_calib[2]); - message.data[3] = static_cast(uss_data.distances_calib[3]); - message.data[4] = static_cast(uss_data.distances_calib[4]); - message.data[5] = static_cast(uss_data.distances_calib[5]); - message.data[6] = static_cast(uss_data.distances_calib[6]); - - message.data[7] = static_cast(uss_data.distances_calib[2]); + message.data[0] = uss_data.distances_calib[0]; + message.data[1] = uss_data.distances_calib[1]; + message.data[2] = uss_data.distances_calib[2]; + message.data[3] = uss_data.distances_calib[3]; + message.data[4] = uss_data.distances_calib[4]; + message.data[5] = uss_data.distances_calib[5]; + message.data[6] = uss_data.distances_calib[6]; + message.data[7] = uss_data.distances_calib[7]; // 发布消息 uss_publisher_->publish(message); // 调试打印 - RCLCPP_INFO( - this->get_logger(), - "USS 1~4: %d %d %d %d cm", - static_cast(uss_data.distances_calib[0]), - static_cast(uss_data.distances_calib[1]), - static_cast(uss_data.distances_calib[2]), - static_cast(uss_data.distances_calib[3])); - RCLCPP_INFO( - this->get_logger(), - "USS 5~8: %d %d %d %d cm", - static_cast(uss_data.distances_calib[4]), - static_cast(uss_data.distances_calib[5]), - static_cast(uss_data.distances_calib[6]), - static_cast(uss_data.distances_calib[7])); + RCLCPP_INFO(this->get_logger(), "USS 1~4: %d %d %d %d cm", static_cast(uss_data.distances_calib[0]), + static_cast(uss_data.distances_calib[1]), static_cast(uss_data.distances_calib[2]), + static_cast(uss_data.distances_calib[3])); + RCLCPP_INFO(this->get_logger(), "USS 5~8: %d %d %d %d cm", static_cast(uss_data.distances_calib[4]), + static_cast(uss_data.distances_calib[5]), static_cast(uss_data.distances_calib[6]), + static_cast(uss_data.distances_calib[7])); RCLCPP_INFO(this->get_logger(), "=========================="); } else @@ -104,7 +94,7 @@ class UssNode : public rclcpp::Node UssDriver uss_driver_; rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr uss_publisher_; + rclcpp::Publisher::SharedPtr uss_publisher_; }; int main(int argc, char** argv)