引言

这篇博客记录学习OriginBot人体跟踪功能的代码过程。主要分为函数调用链路梳理和关键功能解释两部分。

项目结构概览

在开始深入代码之前，我们需要对项目的文件结构有一个整体的认识。originbot人体跟踪功能代码主要包含以下几个部分（在originbot_deeplearning/body_tracking路径下）：

  • CMakeLists.txt：项目的构建配置文件，定义了如何编译项目。
  • src/：包含所有的源代码文件。
  • include/：包含所有的头文件，定义了代码中使用的类和函数接口。
  • launch/：包含launch文件，用于启动ROS节点和配置参数。
  • images/：存放项目中使用的图像资源。

函数调用链路梳理

要理解整个跟踪功能的运行逻辑，我们需要梳理函数之间的调用关系。以下是主要的函数调用链路：

  1. main.cpp：程序的入口点，负责初始化ROS环境和创建TrackingManager实例。
  2. TrackingManager::Instance()：获取TrackingManager的单例实例。
  3. TrackingManager::FeedSmart()：接收并处理人体检测消息。
  4. TrackingManager::RunTrackingStrategy()：执行跟踪策略，包括处理智能消息、更新跟踪角度、控制机器人运动等。
  5. TrackingManager::ProcessSmart()：处理智能消息，更新跟踪信息。
  6. TrackingManager::UpdateTrackAngle()：计算机器人与跟踪目标之间的角度。
  7. TrackingManager::RotateSwitch()：决定是否需要旋转。
  8. TrackingManager::DoRotateMove()：执行旋转移动。
  9. TrackingManager::CancelMove()：取消移动。

这是尝试画的一个函数调用链路流程图：

开始
  |
  v
初始化ROS环境
  |
  v
创建TrackingManager实例
  |
  v
进入主循环
  |
  +---> 检查智能消息队列
  |        |
  |        +---> 若队列非空
  |        |        |
  |        |        +---> 从队列中取出消息
  |        |        |
  |        |        +---> 调用RunTrackingStrategy方法
  |        |        |
  |        |        +---> 进入RunTrackingStrategy方法
  |        |           |
  |        |           +---> 调用ProcessSmart方法处理消息
  |        |           |
  |        |           +---> 若需要更新角度
  |        |           |        |
  |        |           |        +---> 调用UpdateTrackAngle方法
  |        |           |
  |        |           +---> 若需要旋转
  |        |           |        |
  |        |           |        +---> 调用RotateSwitch方法
  |        |           |
  |        |           +---> 若需要移动
  |        |           |        |
  |        |           |        +---> 调用DoRotateMove或CancelMove方法
  |        |           |
  |        |           +---> 返回主循环
  |        |
  |        +---> 若队列为空
  |              |
  |              +---> 等待新消息
  |
  v
退出主循环
  |
  v
结束

整体逻辑梳理

初始化

当程序启动时，main.cpp中的main函数会首先被调用。这里会初始化ROS环境，并通过TrackingManager::Instance()获取跟踪管理器的实例。同时，会创建一个SingleThreadedExecutor来管理ROS节点的执行。

消息订阅与处理

SmartMsgSubscriber类负责订阅人体检测话题，并在接收到消息时调用FeedSmart方法。这个方法将消息添加到TrackingManager的智能消息队列中。

跟踪策略执行

TrackingManager的智能处理线程会不断检查队列。如果队列中有消息，它会调用RunTrackingStrategy方法来处理这些消息。这个方法是整个跟踪逻辑的核心，它负责根据当前的跟踪状态和配置执行相应的策略。

关键代码解释

跟踪管理器类（TrackingManager）

TrackingManager类是整个跟踪功能的核心。它包含了跟踪信息、配置参数和一系列用于处理跟踪逻辑的方法。

实例获取（Instance()）

std::shared_ptr<TrackingManager> TrackingManager::Instance() {
    static std::shared_ptr<TrackingManager> inst = nullptr;
    if (!inst) {
        inst = std::shared_ptr<TrackingManager>(new TrackingManager());
    }
    return inst;
}

这个方法使用单例模式确保TrackingManager只有一个实例。如果实例不存在，它会创建一个新的实例并返回。

消息处理（FeedSmart()）

void TrackingManager::FeedSmart(const ai_msgs::msg::PerceptionTargets::ConstSharedPtr &msg) {
    std::unique_lock<std::mutex> lg(smart_queue_mtx_);
    smart_queue_.push(msg);
    if (smart_queue_.size() > queue_len_limit_) {
        RCLCPP_INFO(rclcpp::get_logger("TrackingManager"),
                    "smart queue len exceed limit: %d", queue_len_limit_);
        smart_queue_.pop();
    }
    smart_queue_condition_.notify_one();
    lg.unlock();
}

FeedSmart方法将接收到的人体检测消息添加到队列中。如果队列长度超过了设定的限制，它会弹出最旧的消息，并通知智能处理线程。

跟踪策略执行（RunTrackingStrategy()）

void TrackingManager::RunTrackingStrategy(
    const ai_msgs::msg::PerceptionTargets::ConstSharedPtr &msg) {
    // ... 跟踪策略逻辑 ...
}

RunTrackingStrategy方法包含了跟踪策略的实现。它会根据当前的跟踪状态和配置，调用其他方法来处理智能消息、更新跟踪角度、控制机器人运动等。

智能消息订阅者类（SmartMsgSubscriber）

SmartMsgSubscriber类负责订阅人体检测话题，并在接收到消息时通知TrackingManager。

订阅与回调（topic_callback）

void SmartMsgSubscriber::topic_callback(
    const ai_msgs::msg::PerceptionTargets::SharedPtr msg) {
    // ... 处理接收到的消息 ...
}

topic_callback是消息订阅的回调函数。当有新的消息到达时，这个函数会被调用。在这里，你可以处理消息内容，例如提取人体目标的信息，并将这些信息传递给TrackingManager。

实现细节理解

跟踪信息更新（ProcessSmart()）

ProcessSmart 方法的主要作用是处理从智能消息订阅者接收到的人体检测消息。它负责更新跟踪管理器中的跟踪信息，并根据这些信息决定跟踪状态。

  1. 「检查跟踪状态」：首先，方法会检查当前的跟踪状态（track_info_.tracking_sta）。如果状态是INITING，表示跟踪尚未开始，需要寻找新的跟踪目标。如果状态是LOST，表示跟踪已丢失，需要重置跟踪信息。

  2. 「寻找跟踪目标」：方法会遍历接收到的消息中的所有目标（targets），并根据配置（track_cfg_）决定是跟踪身体（track_body）还是手势（track_hand）。

  3. 「更新跟踪信息」：如果找到匹配的跟踪目标，方法会更新track_info_中的信息，包括跟踪ID、当前的检测框（present_rect）和手势信息（gesture）。

  4. 「激活手势检测」：如果配置了激活手势（activate_wakeup_gesture），方法会检查是否有激活手势，并相应地更新跟踪信息。

  5. 「跟踪丢失保护」：如果跟踪目标连续丢失超过一定帧数（track_serial_lost_num_thr），跟踪管理器会认为跟踪已丢失，并触发保护策略。

角度计算（UpdateTrackAngle()）

UpdateTrackAngle 方法负责计算机器人与跟踪目标之间的角度。这个角度对于控制机器人的运动方向至关重要。

  1. 「计算角度」：方法会使用CalAngelOfTwoVector函数计算跟踪目标的中心点与机器人之间的夹角。这个夹角用于判断机器人是否需要转向以面向跟踪目标。

  2. 「角度转换」：计算出的角度会被转换为机器人坐标系中的角度，这通常涉及到将角度从度转换为弧度，并根据机器人的坐标系进行调整。

  3. 「更新跟踪信息」：计算出的角度会存储在track_info_中，供后续的移动和旋转控制方法使用。

旋转与移动控制（DoRotateMove() 和 CancelMove()）

DoRotateMove 方法根据计算出的角度控制机器人的旋转。

  1. 「旋转决策」：如果RotateSwitch方法返回true，表示机器人需要旋转，方法会计算旋转的步长和方向。

  2. 「发布旋转指令」：方法会创建一个Twist消息，设置旋转部分的值（angular.z），然后通过RobotCmdVelNode的RobotCtl方法发布这个旋转指令到机器人。

  3. 「记录命令」：方法会更新last_cmdvel_系列变量，记录已发送的旋转指令的类型和步长，以便后续的逻辑判断。

CancelMove 方法用于取消当前的移动或旋转指令。

  1. 「停止移动」：方法会创建一个Twist消息，将线性和旋转速度都设置为零，然后发布这个停止指令。

  2. 「更新状态」：方法会更新last_cmdvel_系列变量，标记为取消状态，并重置旋转和移动的步长。

  3. 「保护策略」：如果跟踪目标丢失，方法可能会触发保护策略，例如停止机器人的运动，以防止机器人在没有目标的情况下继续移动。

其他重要概念

参数配置

TrackingManager的行为可以通过参数进行配置。这些参数包括跟踪阈值、速度、激活手势等。参数可以通过launch文件或参数服务器进行设置。

保护策略

为了防止机器人在跟踪过程中发生碰撞或丢失目标，TrackingManager实现了跟踪丢失保护策略和过度移动保护策略。这些策略确保机器人在跟踪过程中保持安全和有效。

结语

怕什么真理无穷，进一寸有一寸的欢喜（最近是真喜欢这句话）