Merge pull request #100 from jkk-research/97-crptg1-develop-compensat…

…ory-control-model 97 crptg1 develop compensatory control model
jkk-research · Jan 28, 2025 · 7f2ef0c · 7f2ef0c
2 parents 3ff50b2 + 6eea5b7
commit 7f2ef0c
Show file tree

Hide file tree

Showing 70 changed files with 571 additions and 703 deletions.
diff --git a/.gitmodules b/.gitmodules
@@ -22,9 +22,6 @@
 [submodule "crp_VIL/nissan_bringup"]
 	path = crp_VIL/nissan_bringup
 	url = https://github.com/szenergy/nissan_bringup
-[submodule "crp_VIL/pacmod_interface"]
-	path = crp_VIL/pacmod_interface
-	url = https://github.com/tier4/pacmod_interface
 [submodule "crp_VIL/kvaser_interface"]
 	path = crp_VIL/kvaser_interface
 	url = https://github.com/astuff/kvaser_interface.git

diff --git a/crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/CMakeLists.txt b/crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/CMakeLists.txt
@@ -8,12 +8,8 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
 find_package(autoware_control_msgs REQUIRED)
 find_package(autoware_planning_msgs REQUIRED)
-find_package(autoware_vehicle_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
 find_package(crp_msgs REQUIRED)
 
 
@@ -24,13 +20,15 @@ add_executable(ctrl_vehicle_control_lat_compensatory src/ctrl_vehicle_control_la
   src/compensatory_model/compensatory_model.cpp
   src/lib/polynomialCalculator.cpp )
 
-ament_target_dependencies(ctrl_vehicle_control_lat_compensatory  rclcpp geometry_msgs std_msgs autoware_control_msgs autoware_planning_msgs autoware_vehicle_msgs nav_msgs crp_msgs)
+ament_target_dependencies(ctrl_vehicle_control_lat_compensatory  rclcpp autoware_control_msgs autoware_planning_msgs crp_msgs)
 
 install(DIRECTORY launch
-  DESTINATION share/${PROJECT_NAME})
+  DESTINATION share/${PROJECT_NAME}
+)
 
 install(TARGETS
-ctrl_vehicle_control_lat_compensatory
-  DESTINATION lib/${PROJECT_NAME})
+  ctrl_vehicle_control_lat_compensatory
+  DESTINATION lib/${PROJECT_NAME}
+)
 
 ament_package()
diff --git a/...s/ctrl_vehicle_control_lat_compensatory/include/compensatory_model/compensatory_model.hpp b/...s/ctrl_vehicle_control_lat_compensatory/include/compensatory_model/compensatory_model.hpp
@@ -36,14 +36,11 @@ namespace crp
                 bool m_trajInvalid{false};
 
                 // FEEDFORWARD
-                double m_lookAheadDistance{0.0f};
-                double m_targetCurvature{0.0f};
                 double m_targetAccelerationFF{0.0f};
-                double m_targetSteeringAngleFF{0.0f};
 
                 // FEEDBACK
                 double m_targetAccelerationFB{0.0f};
-                double m_targetSteeringAngleFB{0.0f};
+
                 double m_lookAheadDistanceFb{0.0f};
                 double m_posErr{0.0f};
                 double m_posIntegralError{0.0f};
@@ -52,31 +49,27 @@ namespace crp
                 double m_posErrPrev{0.0f};
                 double m_orientationErr{0.0f};
 
+                // RESULT
+                double m_k_superPosition{0.5f};
+
                 double m_targetSteeringAngle_prev{0.0f};
                 double m_actualSteeringAngleLog[2]{0.0f, 0.0f};
 
                 // FILTERS
                 double m_steeringTarget_prev{0.0f};
                 double m_derivativeError_prev{0.0f};
 
-                // KPIs - for debug
-                double m_KPI_c0RMS{0.0f};
-                double m_KPI_c0Mean{0.0f};
-                double m_KPI_c0MS{0.0f};
-                double m_KPI_c0Max{4e8};
-                unsigned long int N{0};
-
                 // METHODS
                 void calculateFeedforward(const ControlInput& input, const ControlParams& params);
                 void calculateFeedback(const ControlInput& input, const ControlParams& params);
+                void superPoseFeedforwardAndFeedback ();
 
-                void calculateSteeringAngle(const ControlInput& input, const ControlParams& params);
+                void calculateSteeringAngle(const ControlInput& input, ControlOutput& output, const ControlParams& params);
 
-                void cutRelevantLocalSnippet();
+                void cutRelevantLocalSnippet(const ControlParams &params);
 
                 void calculateLookAheadPose(const ControlInput& input, const ControlParams& params);
 
-                double steeringInverseDynamics(const double& steeringAngle, const ControlParams& params);
         };
     }
 }

diff --git a/.../ctrl_vehicle_control_lat_compensatory/include/ctrl_vehicle_control/controller_inputs.hpp b/.../ctrl_vehicle_control_lat_compensatory/include/ctrl_vehicle_control/controller_inputs.hpp
@@ -15,34 +15,29 @@ namespace crp
             std::vector<double> path_x;
             std::vector<double> path_y;
             std::vector<double> path_theta;
-            double target_speed{0.0f};
+            double targetSpeed{0.0f};
             double egoPoseGlobal [3]{0.0f};
             double vxEgo{0.0f};
             double egoSteeringAngle{0.0f};
         };
 
         struct ControlParams{
-            double ffGainOffsetGround{1.0f};
-            double ffGainSlope{0.1f};
             double ffLookAheadTime{1.5f};
-            double ffMinLookAheadDistance{0.1f};
             double vehAxleDistance{2.9f};
             double maxAcceleration{3.0f};
-            double maxFFAcceleration{1.0f};
             double steeringAngleLPFilter{0.7f};
             double fbLookAheadTime{0.0f};
             double fbPGain{0.5f};
             double fbDGain{0.1f};
             double fbIGain{0.01f};
-            double fbThetaGain{0.0f};
-            double fbMinLookAheadDistance{0.0f};
             double fbIntegralLimit{3.0f};
-            double invSteerTimeConstant{0.0f}; // todo
-            double invSteerDamping{0.0f}; // todo
             double dT{0.0333f};
             double vxMin{3.0f};
             double trajectory_distance{50.0f};
-            bool debugKPIs{true};
+            double sigma_thetaFP{0.25f};
+            double maxThetaFP{0.3f};
+            double targetAccelerationFF_lpFilterCoeff{0.99f};
+            double targetAccelerationFB_lpFilterCoeff{0.99f};
         };
     }
 }

diff --git a/...y/include/ctrl_vehicle_control_lat_compensatory/ctrl_vehicle_control_lat_compensatory.hpp b/...y/include/ctrl_vehicle_control_lat_compensatory/ctrl_vehicle_control_lat_compensatory.hpp
@@ -1,20 +1,12 @@
-#ifndef CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP
-#define CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP
-
-#include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <math.h>
+#ifndef CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_CTRLVEHILECONTROLLATCOMPENSATORY_HPP
+#define CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_CTRLVEHILECONTROLLATCOMPENSATORY_HPP
+
+
 #include "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/pose.hpp"
-#include "std_msgs/msg/float32_multi_array.hpp"
-#include "std_msgs/msg/float32.hpp"
 #include "crp_msgs/msg/ego.hpp"
 
 #include "autoware_control_msgs/msg/control.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
-#include "autoware_vehicle_msgs/msg/steering_report.hpp"
 
 #include "ctrl_vehicle_control/controller_inputs.hpp"
 #include "ctrl_vehicle_control/controller_outputs.hpp"
@@ -25,10 +17,10 @@ namespace crp
 {
     namespace apl
     {
-        class CtrlVehicleControlLat : public rclcpp::Node
+        class CtrlVehicleControlLatCompensatory : public rclcpp::Node
         {
             public:
-                CtrlVehicleControlLat();
+                CtrlVehicleControlLatCompensatory();
 
             private:
                 // VARIABLES
@@ -43,13 +35,13 @@ namespace crp
 
                 void loop();
 
-                rclcpp::TimerBase::SharedPtr timer_;
+                rclcpp::TimerBase::SharedPtr m_timer_;
                 rclcpp::Publisher<autoware_control_msgs::msg::Lateral>::SharedPtr m_pub_cmd;
 
-                rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr m_traj_sub_;
-                rclcpp::Subscription<crp_msgs::msg::Ego>::SharedPtr m_egoVehicle_sub_;
+                rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr m_sub_traj_;
+                rclcpp::Subscription<crp_msgs::msg::Ego>::SharedPtr m_sub_egoVehicle_;
                 autoware_control_msgs::msg::Lateral m_ctrlCmdMsg;
         }; 
     } // namespace apl
 }
-#endif // CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP
+#endif // CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_CTRLVEHILECONTROLLATCOMPENSATORY_HPP
diff --git a/...l_vehicle_control_lat_compensatory/launch/ctrl_vehicle_control_lat_compensatory.launch.py b/...l_vehicle_control_lat_compensatory/launch/ctrl_vehicle_control_lat_compensatory.launch.py
@@ -2,26 +2,25 @@
 from launch_ros.actions import Node
 
 def generate_launch_description():
-    ld = LaunchDescription()
-
     ctrl_vehicle_control_lat_compensatory = Node(
         package="ctrl_vehicle_control_lat_compensatory",
         executable="ctrl_vehicle_control_lat_compensatory",
         parameters=[
-            {"/ctrl/ffGainOffsetGround": 1.0},
-            {"/ctrl/ffGainSlope": 0.0},
             {"/ctrl/ffLookAheadTime": 0.68},
             {"/ctrl/steeringAngleLPFilter": 0.2},
             {"/ctrl/fbLookAheadTime": 0.6},
             {"/ctrl/fbPGain": 0.5},
             {"/ctrl/fbDGain": 0.1},
             {"/ctrl/fbIGain": 0.0},
-            {"/ctrl/fbMinLookAheadDistance": 0.0},
             {"/ctrl/fbIntegralLimit": 3.0},
-            {"/ctrl/fbThetaGain": 0.0},
+            {"/ctrl/trajectory_distance": 50.0},
+            {"/ctrl/sigma_thetaFP": 0.25},
+            {"/ctrl/maxThetaFP": 0.3},
+            {"/ctrl/targetAccelerationFF_lpFilterCoeff": 0.99},
+            {"/ctrl/targetAccelerationFB_lpFilterCoeff": 0.99},
         ]
     )
 
-    ld.add_action(ctrl_vehicle_control_lat_compensatory)
-
-    return ld
+    return LaunchDescription([
+        ctrl_vehicle_control_lat_compensatory
+    ])
diff --git a/crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/package.xml b/crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/package.xml
@@ -14,12 +14,8 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
-  <depend>std_msgs</depend>
   <depend>autoware_control_msgs</depend>
   <depend>autoware_planning_msgs</depend>
-  <depend>autoware_vehicle_msgs</depend>
-  <depend>nav_msgs</depend>
   <depend>crp_msgs</depend>
 
   <export>