changes after test

jkk-research · Jan 16, 2025 · 4d3a780 · 4d3a780
1 parent 113426e
commit 4d3a780
Show file tree

Hide file tree

Showing 10 changed files with 38 additions and 28 deletions.
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
@@ -8,16 +8,17 @@ def generate_launch_description():
         package="ctrl_vehicle_control_lat_compensatory",
         executable="ctrl_vehicle_control_lat_compensatory",
         parameters=[
-            {"/ctrl/ffGainOffsetGround": 0.66},
+            {"/ctrl/ffGainOffsetGround": 1.0},
             {"/ctrl/ffGainSlope": 0.0},
             {"/ctrl/ffLookAheadTime": 0.68},
             {"/ctrl/steeringAngleLPFilter": 0.2},
             {"/ctrl/fbLookAheadTime": 0.6},
-            {"/ctrl/fbPGain": 0.8},
-            {"/ctrl/fbDGain": 1.1},
+            {"/ctrl/fbPGain": 0.5},
+            {"/ctrl/fbDGain": 0.1},
             {"/ctrl/fbIGain": 0.0},
             {"/ctrl/fbMinLookAheadDistance": 0.0},
-            {"/ctrl/fbIntegralLimit": 3.0}
+            {"/ctrl/fbIntegralLimit": 3.0},
+            {"/ctrl/fbThetaGain": 0.0},
         ]
     )
 

diff --git a/...llers/ctrl_vehicle_control_lat_compensatory/src/compensatory_model/compensatory_model.cpp b/...llers/ctrl_vehicle_control_lat_compensatory/src/compensatory_model/compensatory_model.cpp
@@ -125,7 +125,7 @@ namespace crp
             m_posDerivativeError = (m_posErr-m_posErrPrev)/params.dT;
             m_posDerivativeError = m_posDerivativeFilter.lowPassFilter(m_posDerivativeError, m_posDerivativeError_prev, 0.9f);
 
-            m_targetAccelerationFB = params.fbPGain*m_posErr +
+            m_targetAccelerationFB = params.fbPGain*m_posErr*std::abs(6*m_posErr) +
                 params.fbDGain*m_posDerivativeError;
 
             m_targetAccelerationFB = m_targetAccelerationFB +

diff --git a/...llers/ctrl_vehicle_control_lat_compensatory/src/ctrl_vehicle_control_lat_compensatory.cpp b/...llers/ctrl_vehicle_control_lat_compensatory/src/ctrl_vehicle_control_lat_compensatory.cpp
@@ -12,17 +12,17 @@ crp::apl::CtrlVehicleControlLat::CtrlVehicleControlLat() : Node("CtrlVehicleCont
     m_traj_sub_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>("/plan/trajectory", 10, std::bind(&CtrlVehicleControlLat::trajCallback, this, std::placeholders::_1));
     m_egoVehicle_sub_ = this->create_subscription<crp_msgs::msg::Ego>("/ego", 10, std::bind(&CtrlVehicleControlLat::egoVehicleCallback, this, std::placeholders::_1));
 
-        this->declare_parameter("/ctrl/ffGainOffsetGround", 0.0f);
+        this->declare_parameter("/ctrl/ffGainOffsetGround", 0.66f);
         this->declare_parameter("/ctrl/ffGainSlope", 0.0f);
-        this->declare_parameter("/ctrl/ffLookAheadTime", 0.1f);
+        this->declare_parameter("/ctrl/ffLookAheadTime", 0.68f);
         this->declare_parameter("/ctrl/ffMinLookAheadDistance", 0.0f);
-        this->declare_parameter("/ctrl/steeringAngleLPFilter", 0.5f);
-        this->declare_parameter("/ctrl/fbLookAheadTime", 0.1f);
-        this->declare_parameter("/ctrl/fbPGain", 0.1f);
-        this->declare_parameter("/ctrl/fbDGain", 0.0f);
+        this->declare_parameter("/ctrl/steeringAngleLPFilter", 0.2f);
+        this->declare_parameter("/ctrl/fbLookAheadTime", 0.0f);
+        this->declare_parameter("/ctrl/fbPGain", 0.8f);
+        this->declare_parameter("/ctrl/fbDGain", 1.1f);
         this->declare_parameter("/ctrl/fbIGain", 0.0f);
-        this->declare_parameter("/ctrl/fbThetaGain", 0.0f);
-        this->declare_parameter("/ctrl/fbMinLookAheadDistance", 0.5f);
+        this->declare_parameter("/ctrl/fbThetaGain", 0.05f);
+        this->declare_parameter("/ctrl/fbMinLookAheadDistance", 0.0f);
         this->declare_parameter("/ctrl/fbIntegralLimit", 3.0f);
         this->declare_parameter("/ctrl/trajectory_distance", 50.0f);
         this->declare_parameter("/ctrl/debugKPIs", true);

diff --git a/...ollers/ctrl_vehicle_control_lat_stanley/launch/ctrl_vehicle_control_lat_stanley.launch.py b/...ollers/ctrl_vehicle_control_lat_stanley/launch/ctrl_vehicle_control_lat_stanley.launch.py
@@ -8,7 +8,7 @@ def generate_launch_description():
         package="ctrl_vehicle_control_lat_stanley",
         executable="ctrl_vehicle_control_lat_stanley",
         parameters=[
-            {"/ctrl/k_gain": 0.5},
+            {"/ctrl/k_gain": 1.5},
             {"/ctrl/wheelbase": 2.7},
         ]
     )

diff --git a/crp_APL/controllers/ctrl_vehicle_control_lqr/launch/lat_lon_control_lqr.launch.py b/crp_APL/controllers/ctrl_vehicle_control_lqr/launch/lat_lon_control_lqr.launch.py
@@ -10,10 +10,10 @@ def generate_launch_description():
             output='screen',
             parameters=[{
                 'dt': 0.03,
-                'wheel_base': 1.9,
+                'wheel_base': 2.79,
                 'max_steer_tire_angle': 0.436332,
-                'Q': [0.001, 0.0, 0.001, 0, 0.6], # Q matrix diagonal elements 
-                'R': [30.0, 2.0], # R matrix diagonal elements
+                'Q': [0.005, 0.0, 0.005, 0, 3.5], # Q matrix diagonal elements 
+                'R': [10.0, 1.0], # R matrix diagonal elements
             }]
         )
     ])
diff --git a/crp_APL/planners/plan_lat_lane_follow_ldm/src/wrapperPlanLatLaneFollowLdm.cpp b/crp_APL/planners/plan_lat_lane_follow_ldm/src/wrapperPlanLatLaneFollowLdm.cpp
@@ -26,14 +26,14 @@ void crp::apl::PlanLatLaneFollowHandler::plan(const PlannerInput & input, Planne
 {
     output.trajectory.clear(); // initialize the output at empty vector in the beginning of each cycle
     // TrajectoryPoint trajectoryPoint;
-    /*for (int n=0; n<input.path.pathPoints.size(); n++)
-        {
-            trajectoryPoint.pose.position.x = input.path.pathPoints.at(n).pose.position.x;
-            trajectoryPoint.pose.position.y = input.path.pathPoints.at(n).pose.position.y;
-            trajectoryPoint.pose.orientation = 0.0;
-            trajectoryPoint.velocity = input.path.targetSpeed.at(n);
-            output.trajectory.push_back(trajectoryPoint);
-        }*/
+    // for (int n=0; n<input.path.pathPoints.size(); n++)
+    //     {
+    //         trajectoryPoint.pose.position.x = input.path.pathPoints.at(n).pose.position.x;
+    //         trajectoryPoint.pose.position.y = input.path.pathPoints.at(n).pose.position.y;
+    //         trajectoryPoint.pose.orientation = 0.0;
+    //         trajectoryPoint.velocity = input.path.targetSpeed.at(n);
+    //         output.trajectory.push_back(trajectoryPoint);
+    //     }
 
     // remap parameters
     std::vector<double> nodePointDistances;

diff --git a/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py b/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py
@@ -7,7 +7,7 @@
 def generate_launch_description():
     lanelet2_path_arg = DeclareLaunchArgument(
         'map_file_path',
-        default_value="/home/matyko/lanelet2_maps/auto_mapper/lightweight_lanelet.osm",
+        default_value="/home/dev/lanelet2_maps/auto_mapper/lightweight_lanelet.osm",
         description='Path to the lanelet2 map file'
     )
 

diff --git a/crp_VIL/pacmod_extender/include/pacmod_extender/pacmodExtender.hpp b/crp_VIL/pacmod_extender/include/pacmod_extender/pacmodExtender.hpp
@@ -8,6 +8,8 @@
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <pacmod3_msgs/msg/yaw_rate_rpt.hpp>
 #include <pacmod3_msgs/msg/linear_accel_rpt.hpp>
+#include <std_msgs/msg/float32.hpp>
+
 
 #include "pacmod_extender/pacmodDefinitions.hpp"
 
@@ -29,6 +31,7 @@ class PacmodExtender : public rclcpp::Node
     rclcpp::Publisher<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr m_pub_vehicleAccel_;
     rclcpp::Publisher<pacmod3_msgs::msg::LinearAccelRpt>::SharedPtr m_pub_linAccel_;
     rclcpp::Publisher<pacmod3_msgs::msg::YawRateRpt>::SharedPtr m_pub_yawRate_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr m_pub_tireAngle_;
 
     PacmodDefinitions pacmodDefinitions;
 

diff --git a/crp_VIL/pacmod_extender/src/pacmodExtender.cpp b/crp_VIL/pacmod_extender/src/pacmodExtender.cpp
@@ -10,6 +10,7 @@ PacmodExtender::PacmodExtender() : Node("pacmod_extender_node")
     m_pub_vehicleAccel_ = this->create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>("/sensing/vehicle/accel", 10);
     m_pub_linAccel_     = this->create_publisher<pacmod3_msgs::msg::LinearAccelRpt>("pacmod/linear_accel_rpt", 10);
     m_pub_yawRate_        = this->create_publisher<pacmod3_msgs::msg::YawRateRpt>("pacmod/yaw_rate_calc_rpt", 10);
+    m_pub_tireAngle_    = this->create_publisher<std_msgs::msg::Float32>("/sensing/vehicle/tire_angle", 10);
 
     m_timer_ = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&PacmodExtender::publishMessages, this));
 }
@@ -51,6 +52,11 @@ void PacmodExtender::twistCallback(const geometry_msgs::msg::TwistStamped::Share
     m_twistWithCovariance.twist.twist = msg->twist;
     // twist callback has tire angle in angular.z, the published message should have yaw rate
     m_twistWithCovariance.twist.twist.angular.z = msg->twist.linear.x * tan(msg->twist.angular.z) / WHEELBASE;
+
+    std_msgs::msg::Float32 tireAngleMsg;
+    tireAngleMsg.data = msg->twist.angular.z/14.8;
+
+    m_pub_tireAngle_->publish(tireAngleMsg);
 }
 
 

diff --git a/launch/crp_launcher/launch/lexus.launch.py b/launch/crp_launcher/launch/lexus.launch.py
@@ -12,7 +12,7 @@ def generate_launch_description():
 
     localization_source_arg = DeclareLaunchArgument(
         'localization_source',
-        default_value='ekf',
+        default_value='gnss',
         description='Localization source [ekf or gnss]')
     select_gps_arg = DeclareLaunchArgument(
         'select_gps',
@@ -175,7 +175,7 @@ def generate_launch_description():
     #         join(
     #             get_package_share_directory('crp_launcher'),
     #             'launch',
-    #             'core.pure_p.py')
+    #             'core_pure_p.launch.py')
     #     )
     # )