ROS2机器人应用开发从零到一：那些官方教程没告诉你的"避坑指南"与实战架构

cmake_minimum_required(VERSION 3.8)
project(your_robot_controller)

# 强制使用C++17（ROS2 Humble及以上推荐）
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

# 找依赖包——注意顺序，依赖越多find_package越慢
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)  # 生命周期节点
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(Eigen3 REQUIRED)            # 数学库
find_package(PCL REQUIRED)               # 点云处理

# 添加可执行文件
add_executable(controller_node
  src/controller_node.cpp
  src/pid_controller.cpp
  src/odometry_updater.cpp
)

# 链接依赖库——注意顺序，ament_target_dependencies会自动处理传递依赖
ament_target_dependencies(controller_node
  rclcpp
  rclcpp_lifecycle
  std_msgs
  geometry_msgs
  Eigen3
  PCL
)

# 关键：PCL需要额外链接，ament不会自动带
target_link_libraries(controller_node
  ${PCL_LIBRARIES}
)

# 安装规则
install(TARGETS controller_node
  DESTINATION lib/${PROJECT_NAME}
)

# 别忘了这个——否则colcon test会报错
ament_package()

// 传感器数据——用BEST_EFFORT + KEEP_LAST(1)
rclcpp::SensorDataQoS()  // 这是最优雅的写法
// 等价于：
// reliability = BEST_EFFORT
// durability = VOLATILE
// history = KEEP_LAST(1)

// 控制指令（cmd_vel）——用RELIABLE + KEEP_LAST(1)
rclcpp::SystemDefaultsQoS()
// 等价于：
// reliability = RELIABLE
// durability = VOLATILE
// history = KEEP_LAST(1)

// 静态地图/TF静态变换——用RELIABLE + KEEP_LAST(1) + TRANSIENT_LOCAL
rclcpp::StaticMapQoS()  
// 后加入的节点也能收到"历史"数据

// ❌ 错误示范——这会堵死spin线程
void set_mode_callback(
  const SetMode::Request::SharedPtr req,
  SetMode::Response::SharedPtr res)
{
  // 假设这里有个耗时2秒的硬件握手
  hardware_driver->handshake();  // 阻塞！
  res->success = true;
}

// ✅ 正确姿势——丢给异步线程
void set_mode_callback(
  const SetMode::Request::SharedPtr req,
  SetMode::Response::SharedPtr res)
{
  // 用std::async或线程池执行耗时操作
  std::async(std::launch::async, [this, res]() {
    hardware_driver->handshake();
    res->success = true;
    // 注意：这里需要确保response的生命周期
  });
}

// Action Server反馈示例（C++）
void execute_callback(
  const GoalHandle::SharedPtr goal_handle)
{
  auto feedback = std::make_shared<MoveToPose::Feedback>();
  auto result = std::make_shared<MoveToPose::Result>();
  
  for (int i = 0; i < 100; ++i) {
    // 检查是否被取消
    if (goal_handle->is_canceling()) {
      goal_handle->canceled(result);
      return;
    }
    // 发送进度反馈
    feedback->progress = i;
    goal_handle->publish_feedback(feedback);
    std::this_thread::sleep_for(50ms);
  }
  
  result->success = true;
  goal_handle->succeed(result);
}

rqt_graph --verbose

# 录制（只录关键话题，别什么都录）
ros2 bag record -s mcap /scan /odom /cmd_vel --max-cache-size 100

# 播放（加上循环，方便反复调试）
ros2 bag play -l your_bag.mcap

// 高频回调里只打DEBUG级别
RCLCPP_DEBUG(logger, "Current error: %f", error);

// 或者用ONCE，只打一次
RCLCPP_INFO_ONCE(logger, "Controller initialized");

# 把controller_node的日志级别调整为WARN
ros2 run rqt_logger_level rqt_logger_level
# 或者在命令行直接设置
ros2 param set /controller_node logger_level WARN

#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "lifecycle_msgs/msg/state.hpp"

class RobotController : public rclcpp_lifecycle::LifecycleNode
{
public:
  explicit RobotController(const rclcpp::NodeOptions & options)
  : rclcpp_lifecycle::LifecycleNode("robot_controller", options)
  {}

  // 配置阶段：打开硬件、分配资源
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(get_logger(), "Configuring...");
    // 打开串口、初始化相机、分配内存
    if (!serial_driver_->open("/dev/ttyUSB0", 115200)) {
      return CallbackReturn::FAILURE;
    }
    // 创建订阅/发布（此时不激活）
    cmd_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
      "cmd_vel", 10, std::bind(&RobotController::cmd_callback, this, _1));
    return CallbackReturn::SUCCESS;
  }

  // 激活阶段：使能硬件、开始输出
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_activate(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(get_logger(), "Activating...");
    // 电机使能、开始发送控制指令
    motor_driver_->enable();
    // 激活发布者（LifecyclePublisher需要显式激活）
    cmd_pub_->on_activate();
    return CallbackReturn::SUCCESS;
  }

  // 停用阶段：安全停止
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_deactivate(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(get_logger(), "Deactivating...");
    motor_driver_->disable();  // 紧急停止
    cmd_pub_->on_deactivate();
    return CallbackReturn::SUCCESS;
  }

  // 清理阶段：释放资源
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_cleanup(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(get_logger(), "Cleaning up...");
    serial_driver_->close();
    return CallbackReturn::SUCCESS;
  }

private:
  std::shared_ptr<SerialDriver> serial_driver_;
  std::shared_ptr<MotorDriver> motor_driver_;
  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
};

# 启动节点（默认是Unconfigured状态）
ros2 run your_pkg controller_node

# 查看当前状态
ros2 lifecycle get /robot_controller

# 依次触发状态转换
ros2 lifecycle set /robot_controller configure
ros2 lifecycle set /robot_controller activate
# 停用（比如遇到紧急情况）
ros2 lifecycle set /robot_controller deactivate
# 重新激活（比如故障恢复后）
ros2 lifecycle set /robot_controller activate
# 完全清理
ros2 lifecycle set /robot_controller cleanup

# ❌ 别每次都全量编译
colcon build

# ✅ 只编译你改的那个包
colcon build --packages-select your_controller

# ✅ 显示完整错误信息（别被一堆--淹没）
colcon build --event-handlers console_direct+

# ✅ 多线程编译（根据CPU核心数调整）
colcon build --parallel-workers 8

# ✅ 组合技
colcon build --packages-select your_controller --event-handlers console_direct+ --parallel-workers 8

# 机器人1
ros2 run your_pkg controller_node --ros-args -r __node:=robot1_controller
# 所有话题自动带/robot1前缀

# 机器人2
ros2 run your_pkg controller_node --ros-args -r __node:=robot2_controller

// 核心回调：找最近点，算角度误差
void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
  float min_range = msg->range_max;
  float min_angle = 0.0;
  
  // 只关心正前方±60°范围
  int start_idx = (int)((M_PI/3 - msg->angle_min) / msg->angle_increment);
  int end_idx = (int)((2*M_PI/3 - msg->angle_min) / msg->angle_increment);
  
  for (int i = start_idx; i < end_idx && i < msg->ranges.size(); ++i) {
    if (msg->ranges[i] > msg->range_min && msg->ranges[i] < min_range) {
      min_range = msg->ranges[i];
      min_angle = msg->angle_min + i * msg->angle_increment;
    }
  }
  
  // 计算转向角度（比例控制）
  float error = min_angle;  // 目标在正前方，误差就是角度偏差
  float angular_z = -0.5 * error;  // P控制
  
  // 距离越近，线速度越小（防止撞上）
  float linear_x = std::min(0.3f, 0.1f * min_range);
  
  // 发布cmd_vel
  auto twist = geometry_msgs::msg::Twist();
  twist.linear.x = linear_x;
  twist.angular.z = angular_z;
  cmd_pub_->publish(twist);
}

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    return LaunchDescription([
        # 1. 启动激光雷达驱动（假设是ydlidar）
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([
                '/opt/ros2/share/ydlidar_ros2/launch/ydlidar_launch.py'
            ])
        ),
        
        # 2. 启动跟随控制器（生命周期节点）
        Node(
            package='your_follower',
            executable='follower_node',
            name='follower',
            output='screen',
            parameters=[{'target_distance': 0.5}],
        ),
        
        # 3. 启动Rviz2并加载配置
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', '/path/to/your/follower.rviz'],
        ),
    ])

# 编译
colcon build --packages-select your_follower

# 启动（所有节点同时拉起）
source install/setup.bash
ros2 launch your_follower follower_launch.py

# 在另一个终端，触发生命周期激活
ros2 lifecycle set /follower configure
ros2 lifecycle set /follower activate

# 现在，用手在雷达前面晃动，小车应该跟着转了