uss

src/perception/uss/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.5)
+project(uss)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sweeper_interfaces REQUIRED)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(uss_node
+  src/uss_node.cpp
+  src/uss.cpp
+  src/can_driver.cpp
+)
+
+ament_target_dependencies(uss_node
+  rclcpp
+  std_msgs
+  sweeper_interfaces
+)
+
+install(TARGETS
+  uss_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/perception/uss/config/uss_params.yaml

@@ -0,0 +1,12 @@
+uss_node:
+  ros__parameters:
+    can_interface: "can0"
+    publish_rate: 10
+    sonic_1: 0
+    sonic_2: 0
+    sonic_3: 0
+    sonic_4: 0
+    sonic_5: 0
+    sonic_6: 0
+    sonic_7: 0
+    sonic_8: 0

src/perception/uss/include/can_driver.h

@@ -0,0 +1,70 @@
+#ifndef CAN_DRIVER_H
+#define CAN_DRIVER_H
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <atomic>
+#include <functional>
+#include <string>
+#include <thread>
+#include <vector>
+
+struct CANFrame
+{
+    uint32_t id;
+    uint8_t data[8];
+    uint8_t dlc;
+    bool ext;  // 扩展帧
+    bool rtr;  // 远程帧
+};
+
+class CANDriver
+{
+   public:
+    using ReceiveCallback = std::function<void(const CANFrame&, void*)>;
+
+    CANDriver();
+    ~CANDriver();
+
+    // 打开CAN接口
+    bool open(const std::string& interface);
+
+    // 关闭CAN接口
+    void close();
+
+    // 发送CAN帧
+    bool sendFrame(const CANFrame& frame);
+
+    // 设置接收回调
+    void setReceiveCallback(ReceiveCallback callback, void* userData = nullptr);
+
+    // 设置硬件过滤规则
+    bool setFilter(const std::vector<can_filter>& filters);
+
+    // 追加一个过滤器
+    bool addFilter(const can_filter& filter);
+
+    // 追加一组过滤器
+    bool addFilters(const std::vector<can_filter>& filters);
+
+    // 检查是否已打开
+    bool isOpen() const { return running; }
+
+   private:
+    void receiveThreadFunc();
+    bool applyFilters();  // 应用当前filters_
+
+    int sockfd = -1;
+    std::atomic<bool> running{false};
+    std::thread receiveThread;
+    ReceiveCallback callback;
+    void* userData = nullptr;
+    std::vector<can_filter> filters_;  // 当前所有过滤器
+};
+
+#endif  // CAN_DRIVER_H

src/perception/uss/include/uss.h

@@ -0,0 +1,49 @@
+#ifndef USS_H
+#define USS_H
+
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "can_driver.h"
+
+class UssDriver
+{
+   public:
+    UssDriver();
+    ~UssDriver();
+
+    bool init(const std::string& can_interface = "can0");
+    void close();
+
+    bool isOpen() const { return can_driver_.isOpen(); }
+
+    struct UssData
+    {
+        uint8_t distances[8];        // 8路超声波距离数据 (cm)
+        uint8_t distances_calib[8];  // 8路校准后距离数据 (cm)
+        bool valid;
+    };
+
+    const UssData& getUssData() const;
+
+    void setCalibration(int index, int offset);
+    void setAllCalibrations(const int offsets[8]);
+
+    void setPrintDebug(bool enable) { print_debug_ = enable; }
+
+   private:
+    static void canReceiveCallback(const CANFrame& frame, void* user_data);
+    void processCanFrame(const CANFrame& frame);
+
+    CANDriver can_driver_;
+    UssData uss_data_;
+    mutable std::mutex data_mutex_;
+
+    int calibration_offsets_[8];
+    bool print_debug_;
+    bool initialized_;
+};
+
+#endif  // USS_H

src/perception/uss/launch/uss.launch.py

@@ -0,0 +1,23 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    config = os.path.join(
+        get_package_share_directory('uss'),
+        'config',
+        'uss_params.yaml'
+    )
+
+    return LaunchDescription([
+        Node(
+            package='uss',
+            executable='uss_node',
+            name='uss_node',
+            output='screen',
+            parameters=[config]
+        )
+    ])

src/perception/uss/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>uss</name>
+  <version>0.0.0</version>
+  <description>Ultrasonic Sensor (USS) ROS2 package for sweeper</description>
+  <maintainer email="zxwl@56.com">root</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sweeper_interfaces</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/perception/uss/src/can_driver.cpp

@@ -0,0 +1,176 @@
+#include "can_driver.h"
+
+#include <fcntl.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <poll.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include <cstring>
+#include <iostream>
+#include <stdexcept>
+#include <thread>
+
+CANDriver::CANDriver() = default;
+
+CANDriver::~CANDriver() { close(); }
+
+bool CANDriver::open(const std::string& interface)
+{
+    if (running) return false;
+
+    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    if (sockfd < 0)
+    {
+        perror("socket");
+        return false;
+    }
+
+    struct ifreq ifr;
+    std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ - 1);
+    ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+
+    if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0)
+    {
+        perror("ioctl");
+        ::close(sockfd);
+        return false;
+    }
+
+    struct sockaddr_can addr{};
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    if (bind(sockfd, reinterpret_cast<struct sockaddr*>(&addr), sizeof(addr)) < 0)
+    {
+        perror("bind");
+        ::close(sockfd);
+        return false;
+    }
+
+    // 设置为非阻塞
+    int flags = fcntl(sockfd, F_GETFL, 0);
+    fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);
+
+    running = true;
+    receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
+    return true;
+}
+
+void CANDriver::close()
+{
+    if (!running) return;
+
+    running = false;
+    if (receiveThread.joinable()) receiveThread.join();
+
+    if (sockfd >= 0)
+    {
+        ::close(sockfd);
+        sockfd = -1;
+    }
+}
+
+bool CANDriver::sendFrame(const CANFrame& frame)
+{
+    if (!running) return false;
+
+    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;
+    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;
+}
+
+void CANDriver::setReceiveCallback(ReceiveCallback callback, void* userData)
+{
+    this->callback = callback;
+    this->userData = userData;
+}
+
+bool CANDriver::setFilter(const std::vector<can_filter>& filters)
+{
+    if (!running) return false;
+
+    filters_ = filters;
+    return applyFilters();
+}
+
+bool CANDriver::addFilter(const can_filter& filter)
+{
+    filters_.push_back(filter);
+    return applyFilters();
+}
+
+bool CANDriver::addFilters(const std::vector<can_filter>& filters)
+{
+    filters_.insert(filters_.end(), filters.begin(), filters.end());
+    return applyFilters();
+}
+
+bool CANDriver::applyFilters()
+{
+    if (!running) return false;
+
+    if (filters_.empty())
+    {
+        setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, nullptr, 0);
+        return true;
+    }
+
+    if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, filters_.data(), filters_.size() * sizeof(can_filter)) < 0)
+    {
+        perror("setsockopt");
+        return false;
+    }
+
+    return true;
+}
+
+void CANDriver::receiveThreadFunc()
+{
+    struct can_frame raw_frame;
+    struct pollfd fds;
+    fds.fd = sockfd;
+    fds.events = POLLIN;
+
+    while (running)
+    {
+        int ret = poll(&fds, 1, 100);  // 等待最多100ms
+        if (ret < 0)
+        {
+            perror("poll");
+            continue;
+        }
+        else if (ret == 0)
+        {
+            // 超时无数据，可继续
+            continue;
+        }
+
+        ssize_t nbytes = read(sockfd, &raw_frame, sizeof(raw_frame));
+        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.dlc = raw_frame.can_dlc;
+            memcpy(frame.data, raw_frame.data, raw_frame.can_dlc);
+
+            // 调用回调函数，处理接收到的帧
+            callback(frame, userData);
+        }
+    }
+}

src/perception/uss/src/uss.cpp

@@ -0,0 +1,153 @@
+#include "uss.h"
+
+#include <iostream>
+
+// 超声波传感器的CAN ID定义 (J1939格式: 0x18EC5700)
+// Priority: 0x18, PGN: 0xEC00 (60416), Device ID: 0x57 (87)
+constexpr uint32_t USS_CAN_ID = 0x18EC5700;
+constexpr uint32_t USS_CAN_MASK = 0xFFFFFFFF;  // 精确匹配
+
+UssDriver::UssDriver() : print_debug_(false), initialized_(false)
+{
+    // 初始化校准偏移
+    for (int i = 0; i < 8; i++)
+    {
+        calibration_offsets_[i] = 0;
+    }
+
+    // 初始化数据
+    uss_data_.valid = false;
+    for (int i = 0; i < 8; i++)
+    {
+        uss_data_.distances[i] = 0;
+        uss_data_.distances_calib[i] = 0;
+    }
+}
+
+UssDriver::~UssDriver() { close(); }
+
+bool UssDriver::init(const std::string& can_interface)
+{
+    if (initialized_)
+    {
+        return true;
+    }
+
+    if (!can_driver_.open(can_interface))
+    {
+        std::cerr << "Failed to open CAN interface: " << can_interface << std::endl;
+        return false;
+    }
+
+    // 设置CAN过滤器，只接收超声波的CAN数据
+    struct can_filter filter;
+    filter.can_id = USS_CAN_ID;
+    filter.can_mask = USS_CAN_MASK;
+    std::vector<struct can_filter> filters;
+    filters.push_back(filter);
+
+    if (!can_driver_.setFilter(filters))
+    {
+        std::cerr << "Failed to set CAN filter for USS" << std::endl;
+        can_driver_.close();
+        return false;
+    }
+
+    can_driver_.setReceiveCallback(canReceiveCallback, this);
+    initialized_ = true;
+
+    if (print_debug_)
+    {
+        std::cout << "USS driver initialized successfully on " << can_interface << std::endl;
+        std::cout << "CAN filter set for ID: 0x" << std::hex << USS_CAN_ID << std::dec << std::endl;
+    }
+
+    return true;
+}
+
+void UssDriver::close()
+{
+    if (initialized_)
+    {
+        can_driver_.close();
+        initialized_ = false;
+        if (print_debug_)
+        {
+            std::cout << "USS driver closed" << std::endl;
+        }
+    }
+}
+
+const UssDriver::UssData& UssDriver::getUssData() const
+{
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    return uss_data_;
+}
+
+void UssDriver::setCalibration(int index, int offset)
+{
+    if (index >= 0 && index < 8)
+    {
+        calibration_offsets_[index] = offset;
+        // 更新校准后的数据
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        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)));
+    }
+}
+
+void UssDriver::setAllCalibrations(const int offsets[8])
+{
+    for (int i = 0; i < 8; i++)
+    {
+        calibration_offsets_[i] = offsets[i];
+    }
+
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    for (int i = 0; i < 8; 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)));
+    }
+}
+
+void UssDriver::canReceiveCallback(const CANFrame& frame, void* user_data)
+{
+    if (user_data == nullptr)
+    {
+        return;
+    }
+
+    UssDriver* driver = static_cast<UssDriver*>(user_data);
+    driver->processCanFrame(frame);
+}
+
+void UssDriver::processCanFrame(const CANFrame& frame)
+{
+    if (frame.id == USS_CAN_ID)
+    {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+
+        // 更新原始数据
+        int data_len = std::min(static_cast<int>(frame.dlc), 8);
+        for (int i = 0; i < data_len; i++)
+        {
+            uss_data_.distances[i] = frame.data[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_.valid = true;
+
+        if (print_debug_)
+        {
+            std::cout << "Received USS data: ";
+            for (int i = 0; i < 8; i++)
+            {
+                std::cout << static_cast<int>(uss_data_.distances[i]) << " ";
+            }
+            std::cout << std::endl;
+        }
+    }
+}

src/perception/uss/src/uss_node.cpp

@@ -0,0 +1,109 @@
+#include <iostream>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int8_multi_array.hpp"
+#include "uss.h"
+
+class UssNode : public rclcpp::Node
+{
+   public:
+    UssNode() : Node("uss_node")
+    {
+        RCLCPP_INFO(this->get_logger(), "USS node has been started.");
+
+        // 声明参数
+        this->declare_parameter<std::string>("can_interface", "can0");
+        this->declare_parameter<int>("publish_rate", 10);  // 10Hz
+
+        // 声明超声波校准参数
+        for (int i = 0; i < 8; i++)
+        {
+            this->declare_parameter<int>("sonic_" + std::to_string(i + 1), 0);
+        }
+
+        // 获取参数
+        std::string can_interface = this->get_parameter("can_interface").as_string();
+        int publish_rate = this->get_parameter("publish_rate").as_int();
+
+        // 初始化USS驱动
+        if (!uss_driver_.init(can_interface))
+        {
+            RCLCPP_ERROR(this->get_logger(), "Failed to initialize USS driver on interface %s", can_interface.c_str());
+            return;
+        }
+
+        // 加载超声波校准参数
+        int sonic[8];
+        for (int i = 0; i < 8; i++)
+        {
+            sonic[i] = this->get_parameter("sonic_" + std::to_string(i + 1)).as_int();
+        }
+        uss_driver_.setAllCalibrations(sonic);
+        RCLCPP_INFO(this->get_logger(), "Config loaded successfully");
+
+        // 创建发布者
+        uss_publisher_ = this->create_publisher<std_msgs::msg::Int8MultiArray>("USS", 10);
+
+        // 创建定时器，定期发布数据
+        timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / publish_rate),
+                                         std::bind(&UssNode::timer_callback, this));
+
+        RCLCPP_INFO(this->get_logger(), "USS driver initialized successfully on %s", can_interface.c_str());
+    }
+
+    ~UssNode() { uss_driver_.close(); }
+
+   private:
+    void timer_callback()
+    {
+        auto message = std_msgs::msg::Int8MultiArray();
+        const UssDriver::UssData& uss_data = uss_driver_.getUssData();
+
+        if (uss_data.valid)
+        {
+            message.data.clear();
+            message.data.resize(8);
+
+            message.data[0] = static_cast<int8_t>(uss_data.distances_calib[0]);
+            message.data[1] = static_cast<int8_t>(uss_data.distances_calib[1]);
+            message.data[2] = static_cast<int8_t>(uss_data.distances_calib[2]);
+            message.data[3] = static_cast<int8_t>(uss_data.distances_calib[3]);
+            message.data[4] = static_cast<int8_t>(uss_data.distances_calib[4]);
+            message.data[5] = static_cast<int8_t>(uss_data.distances_calib[5]);
+            message.data[6] = static_cast<int8_t>(uss_data.distances_calib[6]);
+            message.data[7] = static_cast<int8_t>(uss_data.distances_calib[7]);
+
+            // 发布消息
+            uss_publisher_->publish(message);
+
+            // 调试打印
+            for (int i = 0; i < 8; i++)
+            {
+                RCLCPP_INFO(this->get_logger(), "USS %d: %d cm", i + 1, static_cast<int>(message.data[i]));
+            }
+        }
+        else
+        {
+            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No valid USS data received");
+        }
+    }
+
+    UssDriver uss_driver_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<std_msgs::msg::Int8MultiArray>::SharedPtr uss_publisher_;
+};
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<UssNode>();
+
+    if (node)
+    {
+        rclcpp::spin(node);
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}