lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

uss debug

Browse Source
This commit is contained in:
Alvin-lyq 2026-05-06 13:23:28 +08:00
parent 58c05f5563
commit 65d9734daf
3 changed files with 24 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/perception/uss/include/uss.h
View File

@ -21,8 +21,8 @@ class UssDriver
struct UssData struct UssData
{ {
uint8_t distances[8]; // 8路超声波距离数据 (cm) uint8_t distances[8]; // 8路超声波原始距离数据 (cm)
uint8_t distances_calib[8]; // 8路校准后距离数据 (cm) uint16_t distances_calib[8]; // 8路校准后距离数据 (cm)
bool valid; bool valid;
}; };

9
src/perception/uss/src/uss.cpp
View File

@ -6,7 +6,6 @@
// Priority: 0x18, PGN: 0xEC00 (60416), Device ID: 0x57 (87) // Priority: 0x18, PGN: 0xEC00 (60416), Device ID: 0x57 (87)
constexpr uint32_t USS_CAN_ID = 0x18EC0057; constexpr uint32_t USS_CAN_ID = 0x18EC0057;
constexpr uint32_t USS_CAN_MASK = 0xFFFFFFFF; // 精确匹配 constexpr uint32_t USS_CAN_MASK = 0xFFFFFFFF; // 精确匹配
UssDriver::UssDriver() : print_debug_(false), initialized_(false) UssDriver::UssDriver() : print_debug_(false), initialized_(false)
{ {
// 初始化校准偏移 // 初始化校准偏移
@ -42,7 +41,7 @@ bool UssDriver::init(const std::string& can_interface)
// 设置CAN过滤器，只接收超声波的CAN数据 // 设置CAN过滤器，只接收超声波的CAN数据
struct can_filter filter; struct can_filter filter;
filter.can_id = USS_CAN_ID | CAN_EFF_FLAG; 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<struct can_filter> filters; std::vector<struct can_filter> filters;
filters.push_back(filter); filters.push_back(filter);
@ -92,7 +91,7 @@ void UssDriver::setCalibration(int index, int offset)
// 更新校准后的数据 // 更新校准后的数据
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
int calib_distance = static_cast<int>(uss_data_.distances[index]) + offset; int calib_distance = static_cast<int>(uss_data_.distances[index]) + offset;
uss_data_.distances_calib[index] = static_cast<uint8_t>(std::max(0, std::min(255, calib_distance))); uss_data_.distances_calib[index] = static_cast<uint16_t>(std::max(0, calib_distance));
} }
} }
@ -107,7 +106,7 @@ void UssDriver::setAllCalibrations(const int offsets[8])
for (int i = 0; i < 8; i++) for (int i = 0; i < 8; i++)
{ {
int calib_distance = static_cast<int>(uss_data_.distances[i]) + calibration_offsets_[i]; int calib_distance = static_cast<int>(uss_data_.distances[i]) + calibration_offsets_[i];
uss_data_.distances_calib[i] = static_cast<uint8_t>(std::max(0, std::min(255, calib_distance))); uss_data_.distances_calib[i] = static_cast<uint16_t>(std::max(0, calib_distance));
} }
} }
@ -135,7 +134,7 @@ void UssDriver::processCanFrame(const CANFrame& frame)
uss_data_.distances[i] = frame.data[i]; uss_data_.distances[i] = frame.data[i];
// 计算校准后的数据 // 计算校准后的数据
int calib_distance = static_cast<int>(frame.data[i]) + calibration_offsets_[i]; int calib_distance = static_cast<int>(frame.data[i]) + calibration_offsets_[i];
uss_data_.distances_calib[i] = static_cast<uint8_t>(std::max(0, std::min(255, calib_distance))); uss_data_.distances_calib[i] = static_cast<uint16_t>(std::max(0, calib_distance));
} }
uss_data_.valid = true; uss_data_.valid = true;

46
src/perception/uss/src/uss_node.cpp
View File

@ -2,7 +2,7 @@
#include <string> #include <string>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int8_multi_array.hpp" #include "std_msgs/msg/u_int16_multi_array.hpp"
#include "uss.h" #include "uss.h"
class UssNode : public rclcpp::Node class UssNode : public rclcpp::Node
@ -43,7 +43,7 @@ class UssNode : public rclcpp::Node
RCLCPP_INFO(this->get_logger(), "Config loaded successfully"); RCLCPP_INFO(this->get_logger(), "Config loaded successfully");
// 创建发布者 // 创建发布者
uss_publisher_ = this->create_publisher<std_msgs::msg::Int8MultiArray>("USS", 10); uss_publisher_ = this->create_publisher<std_msgs::msg::UInt16MultiArray>("USS", 10);
// 创建定时器，定期发布数据 // 创建定时器，定期发布数据
timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / publish_rate), timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / publish_rate),
@ -57,7 +57,7 @@ class UssNode : public rclcpp::Node
private: private:
void timer_callback() void timer_callback()
{ {
auto message = std_msgs::msg::Int8MultiArray(); auto message = std_msgs::msg::UInt16MultiArray();
const UssDriver::UssData& uss_data = uss_driver_.getUssData(); const UssDriver::UssData& uss_data = uss_driver_.getUssData();
if (uss_data.valid) if (uss_data.valid)
@ -65,35 +65,25 @@ class UssNode : public rclcpp::Node
message.data.clear(); message.data.clear();
message.data.resize(8); message.data.resize(8);
message.data[0] = static_cast<int8_t>(uss_data.distances_calib[0]); message.data[0] = uss_data.distances_calib[0];
message.data[1] = static_cast<int8_t>(uss_data.distances_calib[1]); message.data[1] = uss_data.distances_calib[1];
message.data[2] = uss_data.distances_calib[2];
message.data[2] = static_cast<int8_t>(uss_data.distances_calib[2]); message.data[3] = uss_data.distances_calib[3];
message.data[3] = static_cast<int8_t>(uss_data.distances_calib[3]); message.data[4] = uss_data.distances_calib[4];
message.data[4] = static_cast<int8_t>(uss_data.distances_calib[4]); message.data[5] = uss_data.distances_calib[5];
message.data[5] = static_cast<int8_t>(uss_data.distances_calib[5]); message.data[6] = uss_data.distances_calib[6];
message.data[6] = static_cast<int8_t>(uss_data.distances_calib[6]); message.data[7] = uss_data.distances_calib[7];
message.data[7] = static_cast<int8_t>(uss_data.distances_calib[2]);
// 发布消息 // 发布消息
uss_publisher_->publish(message); uss_publisher_->publish(message);
// 调试打印 // 调试打印
RCLCPP_INFO( RCLCPP_INFO(this->get_logger(), "USS 1~4: %d %d %d %d cm", static_cast<int>(uss_data.distances_calib[0]),
this->get_logger(), static_cast<int>(uss_data.distances_calib[1]), static_cast<int>(uss_data.distances_calib[2]),
"USS 1~4: %d %d %d %d cm", static_cast<int>(uss_data.distances_calib[3]));
static_cast<int8_t>(uss_data.distances_calib[0]), RCLCPP_INFO(this->get_logger(), "USS 5~8: %d %d %d %d cm", static_cast<int>(uss_data.distances_calib[4]),
static_cast<int8_t>(uss_data.distances_calib[1]), static_cast<int>(uss_data.distances_calib[5]), static_cast<int>(uss_data.distances_calib[6]),
static_cast<int8_t>(uss_data.distances_calib[2]), static_cast<int>(uss_data.distances_calib[7]));
static_cast<int8_t>(uss_data.distances_calib[3]));
RCLCPP_INFO(
this->get_logger(),
"USS 5~8: %d %d %d %d cm",
static_cast<int8_t>(uss_data.distances_calib[4]),
static_cast<int8_t>(uss_data.distances_calib[5]),
static_cast<int8_t>(uss_data.distances_calib[6]),
static_cast<int8_t>(uss_data.distances_calib[7]));
RCLCPP_INFO(this->get_logger(), "=========================="); RCLCPP_INFO(this->get_logger(), "==========================");
} }
else else
@ -104,7 +94,7 @@ class UssNode : public rclcpp::Node
UssDriver uss_driver_; UssDriver uss_driver_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::Int8MultiArray>::SharedPtr uss_publisher_; rclcpp::Publisher<std_msgs::msg::UInt16MultiArray>::SharedPtr uss_publisher_;
}; };
int main(int argc, char** argv) int main(int argc, char** argv)