fix tebplanner

- 在 optimal_planner.h 中添加缺失的 node_ 成员变量声明
- 修复 teb_local_planner_ros.cpp 中的异常类型（ControllerException -> PlannerException）

这修复了启动时的符号未定义错误：
undefined symbol: _ZN17teb_local_planner17TebOptimalPlannerC1E...

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

src/rm_decision/src/decision_node.cpp

@@ -25,7 +25,7 @@ DecisionNode::DecisionNode(const rclcpp::NodeOptions &options)
     std::bind(&DecisionNode::data_ref_callback, this, std::placeholders::_1));
 
   data_aim_sub_ = this->create_subscription<rm_msgs::msg::DataAim>(
-    "/vision/aim", 10,
+    "/data_aim", 10,
     std::bind(&DecisionNode::data_aim_callback, this, std::placeholders::_1));
 
   nav_status_sub_ = this->create_subscription<rm_msgs::msg::NavStatus>(

src/rm_nav/README.md

@@ -6,9 +6,9 @@ RoboMaster比赛
 纯mid360激光雷达导航项目
 
 
-ros2 launch rm_nav_bringup bringup_sim.launch.py \
+ros2 launch rm_nav_bringup bringup_real.launch.py \
-    world:=RMUL2026 \
+    world:=RUML \
-    mode:=mapping \
+    mode:=nav \
     lio:=fastlio \
     lio_rviz:=True \
     nav_rviz:=True

src/rm_nav/rm_nav_bringup/config/reality/MID360_config.json

@@ -25,11 +25,11 @@
   },
   "lidar_configs" : [
     {
-      "ip" : "192.168.1.119",
+      "ip" : "192.168.1.190",
       "pcl_data_type" : 1,
       "pattern_mode" : 0,
       "extrinsic_parameter" : {
-        "roll": 0.0,
+        "roll": 180.0,
         "pitch": 0.0,
         "yaw": 0.0,
         "x": 0,

src/rm_nav/rm_nav_bringup/config/reality/segmentation_real.yaml

@@ -7,7 +7,7 @@ ground_segmentation:
     n_bins: 120                 # number of radial bins.
     n_segments: 360             # number of radial segments.
 
-    max_dist_to_line: 0.1       # maximum vertical distance of point to line to be considered ground.
+    max_dist_to_line: 0.06       # maximum vertical distance of point to line to be considered ground.
 
     sensor_height: 0.59         # REAL sensor height above ground.
     min_slope: -0.4             # minimum slope of a ground line.

src/rm_nav/rm_nav_bringup/launch/bringup_real.launch.py

@@ -337,16 +337,6 @@ def generate_launch_description():
         ]
     )
 
-    bringup_fake_vel_transform_node = Node(
-        package='fake_vel_transform',
-        executable='fake_vel_transform_node',
-        output='screen',
-        parameters=[{
-            'use_sim_time': use_sim_time,
-            'spin_speed': 0.0 # rad/s
-        }]
-    )
-
     start_mapping = Node(
         condition = LaunchConfigurationEquals('mode', 'mapping'),
         package='slam_toolbox',
@@ -385,7 +375,6 @@ def generate_launch_description():
     ld.add_action(bringup_pointcloud_to_laserscan_node)
     ld.add_action(bringup_LIO_group)
     ld.add_action(start_localization_group)
-    ld.add_action(bringup_fake_vel_transform_node)
     ld.add_action(start_mapping)
     ld.add_action(start_navigation2)
 

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h

@@ -54,6 +54,7 @@
 #include <std_msgs/msg/color_rgba.hpp>
 
 #include <rclcpp/rclcpp.hpp>
+#include <nav2_util/lifecycle_node.hpp>
 
 #include "teb_local_planner/planner_interface.h"
 #include "teb_local_planner/teb_config.h"

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/optimal_planner.h

@@ -41,6 +41,7 @@
 
 #include <math.h>
 
+#include <nav2_util/lifecycle_node.hpp>
 
 // teb stuff
 #include "teb_local_planner/teb_config.h"
@@ -710,6 +711,7 @@ protected:
   RotType prefer_rotdir_; //!< Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates)
   
   // internal objects (memory management owned)
+  nav2_util::LifecycleNode::SharedPtr node_; //!< ROS2 lifecycle node pointer
   TebVisualizationPtr visualization_; //!< Instance of the visualization class
   TimedElasticBand teb_; //!< Actual trajectory object
   RobotFootprintModelPtr robot_model_; //!< Robot model

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.h

@@ -86,7 +86,7 @@ public:
    * @param nh local rclcpp::Node::SharedPtr
    * @param cfg const reference to the TebConfig class for parameters
    */
-  TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg);
+  TebVisualization(const nav2_util::LifecycleNode::SharedPtr & nh, const TebConfig& cfg);
   
   /** @name Publish to topics */
   //@{

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp

@@ -198,7 +198,11 @@ void TebLocalPlannerROS::configure(
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
   nh_ = parent;
 
-  auto node = nh_.lock();
+  auto node = std::dynamic_pointer_cast<nav2_util::LifecycleNode>(nh_.lock());
+  if (!node) {
+    throw nav2_core::PlannerException(
+      "Failed to cast controller parent node to nav2_util::LifecycleNode");
+  }
   logger_ = node->get_logger();
   clock_ = node->get_clock();
 

src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/visualization.cpp

@@ -61,7 +61,7 @@ void publishPlan(const std::vector<geometry_msgs::msg::PoseStamped>& path,
     pub->publish(gui_path);
 }
 
-TebVisualization::TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg) : nh_(nh), cfg_(&cfg), initialized_(false)
+TebVisualization::TebVisualization(const nav2_util::LifecycleNode::SharedPtr & nh, const TebConfig& cfg) : nh_(nh), cfg_(&cfg), initialized_(false)
 {
 }
 

src/rm_serial_driver/src/rm_serial_driver.cpp

@@ -41,7 +41,7 @@ namespace rm_serial_driver
 
     // 订阅视觉控制指令
     data_ai_sub_ = this->create_subscription<rm_msgs::msg::DataAI>(
-        "/gimbal/vision", 10,
+        "/data_ai", 10,
         std::bind(&RMSerialDriver::data_ai_callback, this, std::placeholders::_1));
 
     // 同时订阅导航控制指令（用于底盘控制）
@@ -52,7 +52,7 @@ namespace rm_serial_driver
     RCLCPP_INFO(get_logger(), "话题配置:");
     RCLCPP_INFO(get_logger(), "  发布: /data_mcu (MCU状态)");
     RCLCPP_INFO(get_logger(), "  发布: /data_ref (裁判系统)");
-    RCLCPP_INFO(get_logger(), "  订阅: /gimbal/vision (视觉控制)");
+    RCLCPP_INFO(get_logger(), "  订阅: /data_aim (视觉控制)");
     RCLCPP_INFO(get_logger(), "  订阅: /cmd_vel_move (导航控制)");
 
     // 打开串口