update:add run step guide

README.md

@@ -1,5 +1,5 @@
 # auto drive misson center webots simulation:
-该分支修复了原LQR算法中矩阵错误问题，并对参数进行调教，
+
 ## 1, `ubuntu20.04`, `ubuntu22.04`
 ## 2, 安装 `webots`
 `https://github.com/cyberbotics/webots/tags`，  版本： `ubuntu20.04： R2023a， ubuntu22.04：R2025a`
@@ -22,19 +22,17 @@ source install/setup.bash
 ```
 ## 4, 运行 webots: 
 终端执行命令： `webots`
-## 5, 运行 webots 与 rviz可视化界面
-`ros2 launch webots_simulation robot_launch.py` ，等待rviz2界面打开后，加载配置文件：` File->Open Config->` 选择  `SweetAiRviz.rviz` ，方便起见，打开后，File->Save Config 下次不用再重复加载。该 rviz界面 左右两侧拉开留出视图空间。(test:1701895544295370752.json)
-测试版本中path_visualizer pkg目前与launch一起启动，其路径代码可在src/webots_simulation/src/PathVisualizer.cpp 中修改file_name实现
-## 6, 模拟 `/start_pose` 服务
+## 5, 运行 rviz2界面
+`ros2 launch webots_simulation robot_launch.py` ，等待rviz2界面打开后，加载配置文件：` File->Open Config->` 选择  `SweetAiRviz.rviz` ，方便起见，打开后，File->Save Config 下次不用再重复加载。该 rviz界面 左右两侧拉开留出视图空间。
+## 6, 运行 path_visualizer
+`ros2 run webots_simulation path_visualizer`  ，提示 ： 请输入路径文件路径:  ，输入`https://test.sweetai.cn:4433/load/locationCmdPath/`中任意一个json文件名即可，如：`1699983696942006272.json`
+## 7, 模拟 `/start_pose` 服务
 另启一个终端，模拟 `/start_pose` 服务，常开，执行脚本(注意路径)：`python3 /home/ada/sweet_ws/install/auto_drive_misson_center/share/auto_drive_misson_center/scripts/mock_start_pose_and_publish_rtk.py `
-## 7, 运行 mission_center 任务节点
+## 8, 运行 mission_center 任务节点
 运行 `ros2 launch auto_drive_misson_center mission_center_launch.py` 启动 任务节点。
-## 8, 请求 /invoke/missionOn 服务
-另启一个终端，执行发送任务命令，这里的任务，必须和 `5` 中的匹配，将1701895544295370752.json内容填充到指令中：
+## 9, 请求 /invoke/missionOn 服务
+另启一个终端，执行发送任务命令，这里的任务，必须和 `6` 中的匹配，将11699983696942006272.json1内容填充到指令中：
 ```shell
-ros2 service call /invoke/missionOn auto_drive_proj_msg/srv/ReqString "{request: '[{\"name\":\"value\",\"value\":\"https://test.sweetai.cn:4433/load/locationCmdPath/1701895544295370752.json\"}]', value: ''}"
+ros2 service call /invoke/missionOn auto_drive_proj_msg/srv/ReqString "{request: '[{\"name\":\"value\",\"value\":\"https://test.sweetai.cn:4433/load/locationCmdPath/1699983696942006272.json\"}]', value: ''}"
 ```
-> 观察任务轨迹以及rviz2右侧panel面板参数
-
-rviz2 panel：
-    ![界面示意图](./rviz2.jpg) 
+> 观察任务轨迹。

SweetAiRviz.rviz

@@ -5,13 +5,15 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /Status1
         - /Grid1
         - /Grid1/Offset1
-        - /TF1
-        - /TF1/Frames1
-        - /Axes1
+        - /SweetRobot1
+        - /HistoryPosePath1
+        - /TextInfo1
+        - /TF1/Tree1
       Splitter Ratio: 0.4790419042110443
-    Tree Height: 774
+    Tree Height: 727
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -23,7 +25,7 @@ Panels:
   - Class: rviz_common/Views
     Expanded:
       - /Current View1
-      - /Back vision1
+      - /ThirdPersonFollower1
     Name: Views
     Splitter Ratio: 0.5
   - Class: sweet_rviz_panel/SweetRvizPanel
@@ -32,9 +34,9 @@ Visualization Manager:
   Class: ""
   Displays:
     - Alpha: 0.699999988079071
-      Cell Size: 2
+      Cell Size: 1
       Class: rviz_default_plugins/Grid
-      Color: 46; 52; 54
+      Color: 180; 180; 174
       Enabled: true
       Line Style:
         Line Width: 0.029999999329447746
@@ -57,13 +59,27 @@ Visualization Manager:
       Topic:
         Depth: 5
         Durability Policy: Volatile
+        Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /rviz_planning_path
       Value: true
     - Class: rviz_default_plugins/Marker
-      Enabled: false
+      Enabled: true
       Name: VehiclePose
+      Namespaces:
+        vehicle: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rviz_vehicle_pose
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: false
+      Name: MarkerArray
       Namespaces:
         {}
       Topic:
@@ -71,7 +87,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /rviz_vehicle_pose
+        Value: /vehicle_pose_array
       Value: false
     - Alpha: 1
       Class: rviz_default_plugins/RobotModel
@@ -86,7 +102,8 @@ Visualization Manager:
         Value: /robot_description
       Enabled: true
       Links:
-        All Links Enabled: true
+        All Links Enabled:
+          {}
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
@@ -95,7 +112,9 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
       Name: SweetRobot
       TF Prefix: ""
       Update Interval: 0
@@ -109,22 +128,24 @@ Visualization Manager:
       Topic:
         Depth: 5
         Durability Policy: Volatile
+        Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /rviz_history_traj
       Value: true
     - Class: rviz_default_plugins/Marker
-      Enabled: false
+      Enabled: true
       Name: TextInfo
       Namespaces:
-        {}
+        vehicle_info: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
+        Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /rviz_text_info
-      Value: false
+      Value: true
     - Class: rviz_default_plugins/TF
       Enabled: true
       Frame Timeout: 15
@@ -134,7 +155,7 @@ Visualization Manager:
           Value: true
         odom:
           Value: true
-      Marker Scale: 3
+      Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
@@ -147,15 +168,15 @@ Visualization Manager:
       Value: true
     - Class: rviz_default_plugins/Axes
       Enabled: true
-      Length: 1
+      Length: 3
       Name: Axes
-      Radius: 0.05000000074505806
+      Radius: 0.10000000149011612
       Reference Frame: odom
       Value: true
   Enabled: true
   Global Options:
-    Background Color: 211; 215; 207
-    Fixed Frame: base_link
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
     Frame Rate: 30
   Name: root
   Tools:
@@ -167,6 +188,9 @@ Visualization Manager:
     - Class: rviz_default_plugins/Measure
       Line color: 128; 128; 0
     - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -196,7 +220,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/ThirdPersonFollower
-      Distance: 11.149712562561035
+      Distance: 28.86780548095703
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -210,11 +234,11 @@ Visualization Manager:
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.10000000149011612
-      Pitch: 0.5697959065437317
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5747960805892944
       Target Frame: base_link
       Value: ThirdPersonFollower (rviz_default_plugins)
-      Yaw: 3.124208688735962
+      Yaw: 2.934211492538452
     Saved:
       - Class: rviz_default_plugins/ThirdPersonFollower
         Distance: 10.54976749420166
@@ -230,76 +254,19 @@ Visualization Manager:
         Focal Shape Fixed Size: true
         Focal Shape Size: 0.05000000074505806
         Invert Z Axis: false
-        Name: Back vision
-        Near Clip Distance: 0.10000000149011612
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
         Pitch: 0.4897962808609009
         Target Frame: base_link
         Value: ThirdPersonFollower (rviz_default_plugins)
         Yaw: 3.124211072921753
-      - Class: rviz_default_plugins/ThirdPersonFollower
-        Distance: 9.699112892150879
-        Enable Stereo Rendering:
-          Stereo Eye Separation: 0.05999999865889549
-          Stereo Focal Distance: 1
-          Swap Stereo Eyes: false
-          Value: false
-        Focal Point:
-          X: 1.0536713600158691
-          Y: -0.3854414224624634
-          Z: -0.37999600172042847
-        Focal Shape Fixed Size: true
-        Focal Shape Size: 0.05000000074505806
-        Invert Z Axis: false
-        Name: Third vision
-        Near Clip Distance: 0.009999999776482582
-        Pitch: 0.2047973871231079
-        Target Frame: base_link
-        Value: ThirdPersonFollower (rviz_default_plugins)
-        Yaw: 5.914216041564941
-      - Class: rviz_default_plugins/ThirdPersonFollower
-        Distance: 8.171782493591309
-        Enable Stereo Rendering:
-          Stereo Eye Separation: 0.05999999865889549
-          Stereo Focal Distance: 1
-          Swap Stereo Eyes: false
-          Value: false
-        Focal Point:
-          X: 1.0536713600158691
-          Y: -0.3854414224624634
-          Z: -0.37999600172042847
-        Focal Shape Fixed Size: true
-        Focal Shape Size: 0.05000000074505806
-        Invert Z Axis: false
-        Name: Front vision
-        Near Clip Distance: 0.009999999776482582
-        Pitch: 0.30479729175567627
-        Target Frame: base_link
-        Value: ThirdPersonFollower (rviz_default_plugins)
-        Yaw: 0.021031875163316727
-      - Class: rviz_default_plugins/FPS
-        Enable Stereo Rendering:
-          Stereo Eye Separation: 0.05999999865889549
-          Stereo Focal Distance: 1
-          Swap Stereo Eyes: false
-          Value: false
-        Invert Z Axis: false
-        Name: Top Vision
-        Near Clip Distance: 0.009999999776482582
-        Pitch: 1.2897965908050537
-        Position:
-          X: 0.6246874332427979
-          Y: -2.938462495803833
-          Z: 28.4678955078125
-        Target Frame: base_link
-        Value: FPS (rviz_default_plugins)
-        Yaw: 1.364442229270935
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1926
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000002d8000006eafc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a00560069006500770073010000006e000002e00000013200fffffffb000000100044006900730070006c006100790073010000035a000003fe0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000035f000006eafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00530077006500650074005200760069007a00500061006e0065006c010000006e000006ea000002aa00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000c800000022cfc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000631000006ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001180000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d0000039e000000c10100001cfa000000010100000002fb0000001c00530077006500650074005200760069007a00500061006e0065006c0100000000ffffffff000000a000fffffffb0000000a0056006900650077007301000009240000035c0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000c800000022cfc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b00000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   SweetRvizPanel:
@@ -308,6 +275,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 3200
-  X: 0
-  Y: 0
+  Width: 1834
+  X: 86
+  Y: 27

auto_drive_misson_center/CMakeLists.txt

@@ -14,7 +14,6 @@ find_package(auto_drive_proj_utils REQUIRED)
 find_package(auto_drive_proj_msg REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(fmt REQUIRED)
-find_package(visualization_msgs REQUIRED)
 
 include_directories(
   include
@@ -67,7 +66,6 @@ ament_target_dependencies(mission_center_node
   builtin_interfaces
   auto_drive_proj_utils
   auto_drive_proj_msg
-  visualization_msgs
 )
 
 ament_target_dependencies(auto_mission_handler_node

auto_drive_misson_center/config/misson_center_params.yaml

@@ -2,7 +2,7 @@ auto_mission_handler_node:
   # parameters
   ros__parameters:
     # redis
-    redis_host: "127.0.0.1" # "127.0.0.1"
+    redis_host: "192.168.1.110" # "127.0.0.1"
     redis_port: 6379
     redis_db: 0
 
@@ -20,13 +20,13 @@ path_follow_handler_node:
   # parameters
   ros__parameters:
     # redis
-    redis_host: "127.0.0.1" # "127.0.0.1"
+    redis_host: "192.168.1.110" # "127.0.0.1"
     redis_port: 6379
     redis_db: 0
 
     device_id: "2072868304711761920"
 
-    controller_type: "lqr"  # "pure_pursuit", "lqr", "pid"
+    controller_type: "pure_pursuit"  # "pure_pursuit", "lqr", "pid"
     
     # logger
     logger_logger_name: "path_follow"
@@ -40,7 +40,7 @@ mission_center_node:
   # parameters
   ros__parameters:
     # redis
-    redis_host: "127.0.0.1" # "127.0.0.1"
+    redis_host: "192.168.1.110" # "127.0.0.1"
     redis_port: 6379
     redis_db: 0
 

auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp

@@ -22,18 +22,17 @@ public:
         B.resize(2, 1); // 控制输入矩阵
         Q.resize(2, 2); // 状态误差的加权矩阵
         R.resize(1, 1); // 控制输入的加权矩阵
-        //B << 0, 1;     // 控制输入矩阵
+        B << -1, 1;     // 控制输入矩阵
 
         // 设计LQR的代价函数矩阵
-        Q << 100, 0, 0, 10; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
+        Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
 
-        R << 50; // 控制输入的加权矩阵：惩罚角速度ω
+        R << 10; // 控制输入的加权矩阵：惩罚角速度ω
     };
     ~LQR_Controller_C() {};
     // LQR求解器函数
     MatrixXd lqr(const MatrixXd &A, const MatrixXd &B, const MatrixXd &Q, const MatrixXd &R)
     {
-       
         MatrixXd P = Q; // 初始化P为Q
         MatrixXd P_prev = MatrixXd::Zero(A.rows(), A.cols());
         for (int i = 0; i < 1000; ++i)
@@ -47,16 +46,12 @@ public:
         MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
         return K;
     };
-    float update(const float alpha, const float linear_val, const float dis, const float kappa)
+    float update(const float alpha, const float linear_val, const float dis)
     {
         // std::cout << "LQR update!" << std::endl;
         logger_->trace("LQR update!");
-        double dt = 0.01;
         MatrixXd A(2, 2);
-        A << 1.0, linear_val * dt,
-               0, 1.0;
-        B << 0.5 * linear_val * dt * dt,
-             dt;
+        A << 0, linear_val, 0, 0;
         Eigen::VectorXd x(2);
 
         x << dis, alpha;
@@ -66,24 +61,8 @@ public:
         //      << K << std::endl;
         std::stringstream ss;
         ss << K;
-        logger_->info("LQR Gain Matrix K:\n{}", ss.str());
-        double omega_feedback = -(K * x)(0, 0);
-            // Curvature feedforward
-        double omega_feedforward = linear_val * kappa;// feedforwward unused temporarily
-
-        double omega = omega_feedback;
-
-        logger_->info(
-            "LQR: dis={}, alpha={}, kappa={}, "
-            "omega_fb={}, omega_ff={}, omega={}",
-            dis,
-            alpha,
-            kappa,
-            omega_feedback,
-            omega_feedforward,
-            omega);
-
-        return omega;
+        logger_->trace("LQR Gain Matrix K:\n{}", ss.str());
+        return -(K * x)(0, 0);
     };
     /*void init_lqr(const MatrixXd B, const MatrixXd Q, const MatrixXd R)
     {
@@ -91,26 +70,6 @@ public:
         this->Q = Q;
         this->R = R;
     }*/
-   float update(const float alpha, const float linear_val, const float dis)
-    {
-        // std::cout << "LQR update!" << std::endl;
-        logger_->trace("LQR update!");
-        double dt = 0.01;
-        MatrixXd A(2, 2);
-        A << 1.0, linear_val*dt,0, 1.0;
-        B << 0.5 * linear_val * dt * dt,dt;
-        Eigen::VectorXd x(2);
-
-        x << dis, alpha;
-        // 求解LQR增益矩阵K
-        MatrixXd K = lqr(A, B, Q, R);
-        // std::cout << "LQR Gain Matrix K:" << std::endl
-        //      << K << std::endl;
-        std::stringstream ss;
-        ss << K;
-        logger_->info("LQR Gain Matrix K:\n{}", ss.str());
-        return -(K * x)(0, 0);
-    };
     void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki,
               const float kd, const float look_ahead_dis, const float kv)
     {
@@ -123,7 +82,7 @@ public:
         B.resize(2, 1); // 控制输入矩阵
         Q.resize(2, 2); // 状态误差的加权矩阵
         R.resize(1, 1); // 控制输入的加权矩阵
-        B << 0, 1;     // 控制输入矩阵
+        B << -1, 1;     // 控制输入矩阵
 
         // 设计LQR的代价函数矩阵
         Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ

auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp

@@ -55,6 +55,7 @@ private:
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_warn_pub_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_error_pub_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr properties_report_pub_;
+
     std::vector<std::pair<int, int>> interpolation_map_;
 
     std::shared_future<typename auto_drive_proj_msg::srv::ReqString::Response::SharedPtr> mission_future_;
@@ -85,11 +86,7 @@ private:
                               std::string& mission_data,
                               std::string& follow_data);
     std::vector<std::vector<double>> interpolatePoints(const std::vector<std::vector<double>>& coordinates,
-                                                      double max_distance);
-    // simulation only
-    std::vector<std::vector<double>> alignPathYaw( const std::vector<std::vector<double>>& coordinates,
-                                                    double vehicle_initial_yaw);
-
+                                                      double max_distance = 0.5);
     int updatePathIndices(const int original_ind, const std::vector<std::pair<int, int>>& interpolation_map);
 
     void handleAsyncResponses();
@@ -148,9 +145,6 @@ private:
     int logger_max_size_;
     int logger_max_files_;
     int logger_set_level_;
-
-    // 
-    double vehicle_initial_yaw_ = 0.0;
 };
 
 } // namespace auto_drive_misson_center

auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp

@@ -45,7 +45,7 @@ public:
 private:
     MainState main_state_{MainState::IDLE};
     bool is_resuming_{false};
-    std::string controller_type_{"lqr"};
+    std::string controller_type_{"pure_pursuit"};
 
     rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;

auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp

@@ -15,7 +15,7 @@ struct PathPoint {
     double x;          // 经度或UTM X坐标
     double y;          // 纬度或UTM Y坐标
     double yaw;        // 朝向角度（弧度）
-    double kappa;  // 曲率
+    
     PathPoint(double x_val = 0.0, double y_val = 0.0, double yaw_val = 0.0)
         : x(x_val), y(y_val), yaw(yaw_val) {}
 };
@@ -60,36 +60,7 @@ public:
         }
         return false;
     }
-    void computePathCurvature()
-    {
-        if (path_points_.size() < 3) {
-            return;
-        }
-        for (size_t i = 1; i < path_points_.size() - 1; ++i)
-        {
-            const auto& p_prev = path_points_[i - 1];
-            const auto& p_curr = path_points_[i];
-            const auto& p_next = path_points_[i + 1];
-            double ds1 = std::hypot(p_curr.x - p_prev.x,p_curr.y - p_prev.y);
-            double ds2 = std::hypot(p_next.x - p_curr.x,p_next.y - p_curr.y);
-            double ds = 0.5 * (ds1 + ds2);
-            if (ds < 1e-6) {
-                path_points_[i].kappa= 0.0;
-                continue;
-            }
-            double dyaw = p_next.yaw - p_prev.yaw;
-            while (dyaw > M_PI)
-                dyaw -= 2.0 * M_PI;
-
-            while (dyaw < -M_PI)
-                dyaw += 2.0 * M_PI;
-            path_points_[i].kappa = dyaw / (ds1 + ds2);
-            logger_->info("path ind:{}, kappa:{}", i, path_points_[i].kappa);
-        }
-        // 首尾点处理
-        path_points_.front().kappa= path_points_[1].kappa;
-        path_points_.back().kappa= path_points_[path_points_.size() - 2].kappa;
-    }
+    
     // 运行一次控制计算
     ControlOutput run(const PathPoint& current_pose) {
         if (!is_initialized_) {
@@ -214,9 +185,7 @@ protected:
     
     // 保护成员变量，子类可以访问
     double mission_speed_{0.0};
-    double turn_radius_{0.3};
-    double max_allowed_speed_{1.0};
-    double wheel_dis_{0.95};
+    double turn_radius_{1.0};
     std::vector<PathPoint> path_points_;
     int current_point_index_{0};
     bool is_initialized_{false};

auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp

@@ -29,33 +29,27 @@ protected:
         // 这里需要根据需求设置合适的参数值
         float max_linear_vel = mission_speed_;
         float max_angular_vel = mission_speed_ / turn_radius_; // 根据转弯半径估算最大角速度
-        float max_angular_vel_theory = max_allowed_speed_/wheel_dis_;
-        max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory);
         float kp = 1.76;  // 需要根据实际情况调整
         float ki = 0.0;
         float kd = 0.8;
-        float look_ahead_dis = 0.6;  // 需要根据实际情况调整
-        float kv = 0.0;   // 需要根据实际情况调整
+        float look_ahead_dis = 0.4;  // 需要根据实际情况调整
+        float kv = 1.0;   // 需要根据实际情况调整
         
         controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
         return true;
     }
-    bool kappa_cal = false;
+    
     ControlOutput onRun(const PathPoint& current_pose) override {
         // 从PathPoint计算所需参数
         float alpha = 0.0;        // 角度误差
         float linear_val = 0.0;   // 线速度
         float dis = 0.0;          // 横向误差
-        float kappa = 0.0;        // 路徑曲率
-        if(!kappa_cal){
-            computePathCurvature();
-            kappa_cal = true;
-        }
-        computeControlParams(current_pose, alpha, linear_val, dis, kappa);
-        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}, kappa:{}", alpha, linear_val, dis,kappa);
+        
+        computeControlParams(current_pose, alpha, linear_val, dis);
+        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
         
         // 调用原有控制器的update方法
-        float angular_velocity = controller_->update(alpha, linear_val, dis, kappa);
+        float angular_velocity = controller_->update(alpha, linear_val, dis);
         logger_->debug("PP output angular={}", angular_velocity);
         
         // 判断是否完成
@@ -71,8 +65,8 @@ protected:
 private:
     std::shared_ptr<LQR_Controller_C> controller_;
     std::shared_ptr<spdlog::logger> logger_;
-
-    void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis, float& kappa) {
+    
+    void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis) {
         // 获取当前路径点的索引
         int idx = current_point_index_;
         if (idx >= static_cast<int>(path_points_.size()) - 1) {
@@ -88,15 +82,13 @@ private:
         dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
         
         // 角度误差
-        alpha = static_cast<float>(current_pose.yaw - path_yaw);
+        alpha = static_cast<float>(path_yaw - current_pose.yaw);
         // 归一化到 [-pi, pi]
         while (alpha > M_PI) alpha -= 2 * M_PI;
         while (alpha < -M_PI) alpha += 2 * M_PI;
         
         // 线速度
         linear_val = static_cast<float>(mission_speed_);
-        kappa = path_points_[idx].kappa;
-    
     }
 };
 
@@ -175,36 +167,27 @@ protected:
     bool onInitialize() override {
         float max_linear_vel = mission_speed_;
         float max_angular_vel = mission_speed_ / turn_radius_;
-        float max_angular_vel_theory = max_allowed_speed_/wheel_dis_;
-        max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory);
         float kp = 1.76;
         float ki = 0.0;
         float kd = 0.8;
         float look_ahead_dis = 0.4;   // 前视距离，需要根据实际情况调整
-        float kv = 0.6;               // 速度系数，需要根据实际情况调整
         adapter_look_ahead_dis_ = look_ahead_dis;
-        logger_->debug("PP para mission_speed_={}, turn_radius_={} frontdis={}", mission_speed_, turn_radius_,adapter_look_ahead_dis_ );
-        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, adapter_look_ahead_dis_, kv);
+        float kv = 1.0;               // 速度系数，需要根据实际情况调整
+        
+        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
         return true;
     }
-    bool kappa_cal = false;
+    
     ControlOutput onRun(const PathPoint& current_pose) override {
         float alpha = 0.0;
         float linear_val = 0.0;
         float dis = 0.0;
-        float lon_dis = 0.0;
-        float kappa = 0.0;
-        if(!kappa_cal){
-            computePathCurvature();
-            kappa_cal = true;
-        }
-            
-        // Pure Pursuit使用前视点，计算需要特殊处理
-        computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis, lon_dis, kappa);
-        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}, lon_dis={},kappa={}", alpha, linear_val, dis,lon_dis,kappa);
         
-        float angular_velocity = controller_->update(alpha, linear_val, dis, lon_dis, kappa);
-        //float angular_velocity = controller_->update(alpha, linear_val, dis);
+        // Pure Pursuit使用前视点，计算需要特殊处理
+        computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis);
+        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
+        
+        float angular_velocity = controller_->update(alpha, linear_val, dis);
         logger_->debug("PP output angular={}", angular_velocity);
         bool completed = isPathCompleted(current_pose);
         
@@ -221,7 +204,7 @@ private:
     std::shared_ptr<spdlog::logger> logger_;
     
     void computeControlParamsWithLookAhead(const PathPoint& current_pose, 
-                                          float& alpha, float& linear_val, float& dis, float& lon_dis, float& kappa) {
+                                          float& alpha, float& linear_val, float& dis) {
         // 查找前视点
         PathPoint lookahead_point;
         if (!findLookAheadPoint(current_pose, lookahead_point)) {
@@ -254,46 +237,7 @@ private:
         double dx_path = current_pose.x - path_points_[idx].x;
         double dy_path = current_pose.y - path_points_[idx].y;
         double path_yaw = path_points_[idx].yaw;
-        //dis = static_cast<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
-        dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
-        lon_dis = static_cast<float>(sqrt(dx * dx + dy * dy));
-        kappa = static_cast<float>(path_points_[idx].kappa);
-        logger_->info(
-            "PurePursuit parameters:\n"
-            "  current_x:{}, current_y:{}, current_yaw:{}\n"
-            "  current_path_x:{}, current_path_y:{}, current_path_yaw:{}\n"
-            "  lookahead_x:{}, lookahead_y:{}, lookahead_yaw:{}, lookahead_kappa:{}\n"
-            "  dx:{}, dy:{}, target_yaw:{}, alpha:{}\n"
-            "  linear_val:{}, dis:{}, lon_dis:{}\n"
-            "  current_point_index:{}, idx:{}, path_size:{}",
-            
-            current_pose.x,
-            current_pose.y,
-            current_pose.yaw,
-
-            path_points_[current_point_index_].x,
-            path_points_[current_point_index_].y,
-            path_points_[current_point_index_].yaw,
-
-            lookahead_point.x,
-            lookahead_point.y,
-            lookahead_point.yaw,
-            lookahead_point.kappa,
-
-            dx,
-            dy,
-            target_yaw,
-            alpha,
-
-            linear_val,
-            dis,
-            lon_dis,
-
-            current_point_index_,
-            idx,
-            path_points_.size()
-        );
-                
+        dis = static_cast<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
     }
     
     bool findLookAheadPoint(const PathPoint& current_pose, PathPoint& lookahead_point) {
@@ -323,7 +267,6 @@ private:
                 lookahead_point.x = path_points_[i].x + ratio * dx;
                 lookahead_point.y = path_points_[i].y + ratio * dy;
                 lookahead_point.yaw = path_points_[i].yaw;
-                lookahead_point.kappa = path_points_[i].kappa;
                 return true;
             }
             accumulated_dist += segment_length;

auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp

@@ -27,24 +27,12 @@ public:
         lookAheadDis = look_ahead_dis;
         k_v = kv;
     }
+
     float update(const float alpha, const float linear_val, const float dis)
     {
         // std::cout<<"PurePursuit update!"<<std::endl;
         logger_->trace("PurePursuit update!");
-        float w_val = (2 * fabs(linear_val) * sin(alpha) / (lookAheadDis));  
-        w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
-        // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
-        logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);
-        return w_val;
-    };
-    float update(const float alpha, const float linear_val, const float dis, const float lon_dis,const float kappa)
-    {
-        // std::cout<<"PurePursuit update!"<<std::endl;
-        logger_->trace("PurePursuit update!");
-        logger_->info("PurePursuit process! linear_val:{}, dis:{}, lon_dis:{}, kappa:{}", linear_val, dis, lon_dis, kappa);
-        float w_val = (2 * fabs(linear_val) * sin(alpha) / (lon_dis)) + dis; 
-        
-        //float w_val = linear_val*kappa;  
+        float w_val = (2 * fabs(linear_val) * sin(alpha) / (k_v * fabs(linear_val) + lookAheadDis));  
         w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
         // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
         logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);

auto_drive_misson_center/package.xml

@@ -12,7 +12,6 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
-  <depend>visualization_msgs</depend>
   <depend>auto_drive_proj_utils</depend>
   <depend>auto_drive_proj_msg</depend>
   <depend>libcurl-dev</depend>

auto_drive_misson_center/scripts/mock_start_pose_and_publish_rtk.py

@@ -86,7 +86,7 @@ def main(args=None):
     # 解析命令行参数，注意：分离ros args和自定义argparse参数
     parser = argparse.ArgumentParser()
     parser.add_argument("--url", type=str,
-                        default="https://test.sweetai.cn:4433/load/locationCmdPath/1700080420079468544.json",
+                        default="https://test.sweetai.cn:4433/load/locationCmdPath/1701148411059699712.json",
                         help="geojson path url")
     parsed_args, ros_args = parser.parse_known_args(args)
 
@@ -119,4 +119,4 @@ if __name__ == "__main__":
 3. `/start_pose`服务不解析请求参数，任何入参直接返回`success=True`，完全匹配你的需求。
 4. frame_id 写为`map`，你可以根据实际工程改成`enu`。
 5. 发布周期`timer_period = 0.2`，可按需调快 / 调慢。
-'''
+'''

auto_drive_misson_center/src/mission_center_node.cpp

@@ -427,7 +427,7 @@ void MissionCenterNode::missionOnCallback(
         response->success = true;
         response->message = "Mission request accepted, processing in background";
         LOG_INFO("mission on cb, set main state to WAIT MIS RSP, Mission request accepted, processing in background");
-        LOG_INFO("========================== /invoke/missionOn request=  end");
+
     } catch (const std::exception& e) {
         response->success = false;
         response->message = std::string("Exception: ") + e.what();
@@ -744,67 +744,6 @@ std::string MissionCenterNode::downloadFile(const std::string& url)
     return readBuffer;
 }
 
-std::vector<std::vector<double>> MissionCenterNode::alignPathYaw(
-    const std::vector<std::vector<double>>& coordinates,
-    double vehicle_initial_yaw)
-{
-    std::vector<std::vector<double>> aligned_coordinates;
-
-    if (coordinates.empty())
-        return aligned_coordinates;
-
-    // Path 起点
-    const double origin_x = coordinates.front()[0];
-    const double origin_y = coordinates.front()[1];
-
-    // Path 原始起始航向
-    const double path_initial_yaw = coordinates.front()[2];
-
-    // 将 Path 初始航向旋转到车辆初始航向
-    const double yaw_offset =
-        vehicle_initial_yaw - path_initial_yaw;
-
-    const double cos_offset = std::cos(yaw_offset);
-    const double sin_offset = std::sin(yaw_offset);
-
-    aligned_coordinates.reserve(coordinates.size());
-
-    for (const auto& coord : coordinates)
-    {
-        if (coord.size() < 3)
-            continue;
-
-        const double dx = coord[0] - origin_x;
-        const double dy = coord[1] - origin_y;
-
-        const double x =
-            cos_offset * dx - sin_offset * dy;
-
-        const double y =
-            sin_offset * dx + cos_offset * dy;
-
-        double yaw = coord[2] + yaw_offset;
-
-        while (yaw > M_PI)
-            yaw -= 2.0 * M_PI;
-
-        while (yaw < -M_PI)
-            yaw += 2.0 * M_PI;
-
-        aligned_coordinates.push_back({
-            x,
-            y,
-            yaw
-        });
-        LOG_INFO(
-        "Path new: x={} y={} yaw={} deg",
-        x,
-        y,
-        yaw * 180.0 / M_PI);
-    }
-
-    return aligned_coordinates;
-}
 bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data,
                                             std::string& mission_data, std::string& follow_data)
 {
@@ -847,10 +786,8 @@ bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data
         }
 
         interpolation_map_.clear();
+        std::vector<std::vector<double>> interpolated_coords = interpolatePoints(coordinates, 0.5);
 
-        std::vector<std::vector<double>> aligned_coords = alignPathYaw(coordinates, vehicle_initial_yaw_);
-        std::vector<std::vector<double>> interpolated_coords = interpolatePoints(aligned_coords, 0.2);
-    
         json processed_data = data;
         json new_coordinates = json::array();
         for (const auto& coord : interpolated_coords) {
@@ -920,28 +857,9 @@ std::vector<std::vector<double>> MissionCenterNode::interpolatePoints(
                 double ratio = static_cast<double>(j) / num_segments;
                 double x = current[0] + dx * ratio;
                 double y = current[1] + dy * ratio;
-
-                 // yaw 插值
-                double yaw_diff = next[2] - current[2];
-                // normalize yaw_diff 到 [-pi, pi]
-                while (yaw_diff > M_PI) {
-                    yaw_diff -= 2.0 * M_PI;
-                }
-                while (yaw_diff < -M_PI) {
-                    yaw_diff += 2.0 * M_PI;
-                }
-                double yaw =current[2] + yaw_diff * ratio;
-                // normalize yaw 到 [-pi, pi]
-                while (yaw > M_PI) {
-                    yaw -= 2.0 * M_PI;
-                }
-                while (yaw < -M_PI) {
-                    yaw += 2.0 * M_PI;
-                }
-
-                    result.push_back({x, y, yaw});
-                    inserted_points++;
-                }
+                result.push_back({x, y, current[2]});
+                inserted_points++;
+            }
             interpolation_map_.push_back({i + 1, inserted_points});
         }
 

auto_drive_misson_center/src/path_follow_handler.cpp

@@ -79,7 +79,7 @@ PathFollowHandler::PathFollowHandler(const std::string & node_name)
         10
     );
 
-    this->declare_parameter<std::string>("controller_type", "lqr");
+    this->declare_parameter<std::string>("controller_type", "pure_pursuit");
     this->get_parameter("controller_type", controller_type_);
     LOG_INFO("Controller type parameter: {}", controller_type_);
 
@@ -417,7 +417,7 @@ void PathFollowHandler::processRunningState()
         auto path_ind_msg = std_msgs::msg::Int32();
         path_ind_msg.data = path_controller_->getCurrentPointIndex() + path_ind_start_;
         path_ind_pub_->publish(path_ind_msg);
-        LOG_INFO("Published final path index 1: {}", path_ind_msg.data);
+
         saveCurrentStateToRedis();
 
         LOG_DEBUG("path controller, current pose point:x:{}, y:{}, yaw:{}; twist_msg pub:linear x:{}, angular z:{}, run is completed:{}, Current Point Index:{}, Path Point Count:{}", 
@@ -444,7 +444,7 @@ void PathFollowHandler::processRunningState()
                 }
                 final_ind_msg.data = total_points - 1;
                 path_ind_pub_->publish(final_ind_msg);
-                LOG_INFO("Published final path index 2: {}", final_ind_msg.data);
+
                 LOG_INFO("main state change(Running State:Mission completed):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
                 main_state_ = MainState::AUTO_OFF;
             } else {