Tune lqr control algorithm

1.fix pp algorithm

2.fix point interpolate yaw

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,17 +22,19 @@ source install/setup.bash
 ```
 ## 4, 运行 webots: 
 终端执行命令： `webots`
-## 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` 服务
+## 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` 服务
 另启一个终端，模拟 `/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 `
-## 8, 运行 mission_center 任务节点
+## 7, 运行 mission_center 任务节点
 运行 `ros2 launch auto_drive_misson_center mission_center_launch.py` 启动 任务节点。
-## 9, 请求 /invoke/missionOn 服务
-另启一个终端，执行发送任务命令，这里的任务，必须和 `6` 中的匹配，将11699983696942006272.json1内容填充到指令中：
+## 8, 请求 /invoke/missionOn 服务
+另启一个终端，执行发送任务命令，这里的任务，必须和 `5` 中的匹配，将1701895544295370752.json内容填充到指令中：
 ```shell
-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: ''}"
+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: ''}"
 ```
-> 观察任务轨迹。
+> 观察任务轨迹以及rviz2右侧panel面板参数
+
+rviz2 panel：
+    ![界面示意图](./rviz2.jpg) 

src/auto_drive_misson_center/CMakeLists.txt

@@ -14,6 +14,7 @@ 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
@@ -66,6 +67,7 @@ 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

src/auto_drive_misson_center/config/misson_center_params.yaml

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

src/auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp

@@ -22,17 +22,18 @@ public:
         B.resize(2, 1); // 控制输入矩阵
         Q.resize(2, 2); // 状态误差的加权矩阵
         R.resize(1, 1); // 控制输入的加权矩阵
-        B << -1, 1;     // 控制输入矩阵
+        //B << 0, 1;     // 控制输入矩阵
 
         // 设计LQR的代价函数矩阵
-        Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
+        Q << 100, 0, 0, 10; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
 
-        R << 10; // 控制输入的加权矩阵：惩罚角速度ω
+        R << 50; // 控制输入的加权矩阵：惩罚角速度ω
     };
     ~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)
@@ -46,12 +47,16 @@ 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)
+    float update(const float alpha, const float linear_val, const float dis, const float kappa)
     {
         // std::cout << "LQR update!" << std::endl;
         logger_->trace("LQR update!");
+        double dt = 0.01;
         MatrixXd A(2, 2);
-        A << 0, linear_val, 0, 0;
+        A << 1.0, linear_val * dt,
+               0, 1.0;
+        B << 0.5 * linear_val * dt * dt,
+             dt;
         Eigen::VectorXd x(2);
 
         x << dis, alpha;
@@ -61,8 +66,24 @@ public:
         //      << K << std::endl;
         std::stringstream ss;
         ss << K;
-        logger_->trace("LQR Gain Matrix K:\n{}", ss.str());
-        return -(K * x)(0, 0);
+        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;
     };
     /*void init_lqr(const MatrixXd B, const MatrixXd Q, const MatrixXd R)
     {
@@ -70,6 +91,26 @@ 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)
     {
@@ -82,7 +123,7 @@ public:
         B.resize(2, 1); // 控制输入矩阵
         Q.resize(2, 2); // 状态误差的加权矩阵
         R.resize(1, 1); // 控制输入的加权矩阵
-        B << -1, 1;     // 控制输入矩阵
+        B << 0, 1;     // 控制输入矩阵
 
         // 设计LQR的代价函数矩阵
         Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ

src/auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp

@@ -55,7 +55,6 @@ 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_;
@@ -86,7 +85,11 @@ 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 = 0.5);
+                                                      double max_distance);
+    // simulation only
+    std::vector<std::vector<double>> alignPathYaw( const std::vector<std::vector<double>>& coordinates,
+                                                    double vehicle_initial_yaw);
+
     int updatePathIndices(const int original_ind, const std::vector<std::pair<int, int>>& interpolation_map);
 
     void handleAsyncResponses();
@@ -145,6 +148,9 @@ private:
     int logger_max_size_;
     int logger_max_files_;
     int logger_set_level_;
+
+    // 
+    double vehicle_initial_yaw_ = 0.0;
 };
 
 } // namespace auto_drive_misson_center

src/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_{"pure_pursuit"};
+    std::string controller_type_{"lqr"};
 
     rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;

src/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,7 +60,36 @@ 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_) {
@@ -185,7 +214,9 @@ protected:
     
     // 保护成员变量，子类可以访问
     double mission_speed_{0.0};
-    double turn_radius_{1.0};
+    double turn_radius_{0.3};
+    double max_allowed_speed_{1.0};
+    double wheel_dis_{0.95};
     std::vector<PathPoint> path_points_;
     int current_point_index_{0};
     bool is_initialized_{false};

src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp

@@ -29,27 +29,33 @@ 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.4;  // 需要根据实际情况调整
-        float kv = 1.0;   // 需要根据实际情况调整
+        float look_ahead_dis = 0.6;  // 需要根据实际情况调整
+        float kv = 0.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;          // 横向误差
-        
-        computeControlParams(current_pose, alpha, linear_val, dis);
-        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
+        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);
         
         // 调用原有控制器的update方法
-        float angular_velocity = controller_->update(alpha, linear_val, dis);
+        float angular_velocity = controller_->update(alpha, linear_val, dis, kappa);
         logger_->debug("PP output angular={}", angular_velocity);
         
         // 判断是否完成
@@ -65,8 +71,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) {
+
+    void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis, float& kappa) {
         // 获取当前路径点的索引
         int idx = current_point_index_;
         if (idx >= static_cast<int>(path_points_.size()) - 1) {
@@ -82,13 +88,15 @@ private:
         dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
         
         // 角度误差
-        alpha = static_cast<float>(path_yaw - current_pose.yaw);
+        alpha = static_cast<float>(current_pose.yaw - path_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;
+    
     }
 };
 
@@ -167,27 +175,36 @@ 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;
-        float kv = 1.0;               // 速度系数，需要根据实际情况调整
-        
-        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
+        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);
         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);
-        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
+        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);
+        float angular_velocity = controller_->update(alpha, linear_val, dis, lon_dis, kappa);
+        //float angular_velocity = controller_->update(alpha, linear_val, dis);
         logger_->debug("PP output angular={}", angular_velocity);
         bool completed = isPathCompleted(current_pose);
         
@@ -204,7 +221,7 @@ private:
     std::shared_ptr<spdlog::logger> logger_;
     
     void computeControlParamsWithLookAhead(const PathPoint& current_pose, 
-                                          float& alpha, float& linear_val, float& dis) {
+                                          float& alpha, float& linear_val, float& dis, float& lon_dis, float& kappa) {
         // 查找前视点
         PathPoint lookahead_point;
         if (!findLookAheadPoint(current_pose, lookahead_point)) {
@@ -237,7 +254,46 @@ 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_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()
+        );
+                
     }
     
     bool findLookAheadPoint(const PathPoint& current_pose, PathPoint& lookahead_point) {
@@ -267,6 +323,7 @@ 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;

src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp

@@ -27,12 +27,24 @@ 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) / (k_v * fabs(linear_val) + lookAheadDis));  
+        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;  
         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);

src/auto_drive_misson_center/package.xml

@@ -12,6 +12,7 @@
   <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>

src/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/1701148411059699712.json",
+                        default="https://test.sweetai.cn:4433/load/locationCmdPath/1700080420079468544.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`，可按需调快 / 调慢。
-'''
+'''

src/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,6 +744,67 @@ 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)
 {
@@ -786,8 +847,10 @@ 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) {
@@ -857,9 +920,28 @@ 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;
-                result.push_back({x, y, current[2]});
-                inserted_points++;
-            }
+
+                 // 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++;
+                }
             interpolation_map_.push_back({i + 1, inserted_points});
         }
 

src/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", "pure_pursuit");
+    this->declare_parameter<std::string>("controller_type", "lqr");
     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 {