transform path and optimize control algorithm

Readme.md

@@ -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,7 +20,7 @@ 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
 
@@ -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/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_tracking_controller.hpp

@@ -185,7 +185,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

@@ -167,14 +167,16 @@ 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;
     }
     

src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp

@@ -33,6 +33,7 @@ public:
         // 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);

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) {