94 crptg1 code quality increase

crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/CMakeLists.txt

@@ -22,7 +22,9 @@ add_executable(ctrl_vehicle_control_lat_compensatory src/ctrl_vehicle_control_la
 
 ament_target_dependencies(ctrl_vehicle_control_lat_compensatory  rclcpp autoware_control_msgs autoware_planning_msgs crp_msgs)
 
-install(DIRECTORY launch
+install(DIRECTORY
+  config
+  launch
   DESTINATION share/${PROJECT_NAME}
 )
 

crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/config/ctrlCompensatoryParams.yaml

@@ -0,0 +1,14 @@
+ctrl_vehicle_control_lat_compensatory:
+  ros__parameters:
+    /ctrl/compensatory/ffLookAheadTime:                    0.68
+    /ctrl/compensatory/steeringAngleLPFilter:              0.2
+    /ctrl/compensatory/fbLookAheadTime:                    0.6
+    /ctrl/compensatory/fbPGain:                            0.5
+    /ctrl/compensatory/fbDGain:                            0.1
+    /ctrl/compensatory/fbIGain:                            0.0
+    /ctrl/compensatory/fbIntegralLimit:                    3.0
+    /ctrl/compensatory/trajectory_distance:                50.0
+    /ctrl/compensatory/sigma_thetaFP:                      0.25
+    /ctrl/compensatory/maxThetaFP:                         0.3
+    /ctrl/compensatory/targetAccelerationFF_lpFilterCoeff: 0.99
+    /ctrl/compensatory/targetAccelerationFB_lpFilterCoeff: 0.99

crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/launch/ctrl_vehicle_control_lat_compensatory.launch.py

@@ -1,26 +1,36 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+from os.path import join
+
 
 def generate_launch_description():
+    # ARGUMENTS
+
+    ctrlCompensatoryConfigFileArg = DeclareLaunchArgument(
+        'ctrlCompensatoryConfigFile',
+        default_value=join(
+            get_package_share_directory('ctrl_vehicle_control_lat_compensatory'),
+            'config',
+            'ctrlCompensatoryParams.yaml'
+        ),
+        description='Path to the compensatory control configuration file'
+    )
+
+    # NODES
+
     ctrl_vehicle_control_lat_compensatory = Node(
         package="ctrl_vehicle_control_lat_compensatory",
         executable="ctrl_vehicle_control_lat_compensatory",
         parameters=[
-            {"/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/fbIntegralLimit": 3.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},
+            LaunchConfiguration('ctrlCompensatoryConfigFile'),
         ]
     )
 
     return LaunchDescription([
+        ctrlCompensatoryConfigFileArg,
+
         ctrl_vehicle_control_lat_compensatory
     ])

crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/src/compensatory_model/compensatory_model.cpp

@@ -29,7 +29,7 @@ namespace crp
             if (input.path_x.size() >= 4)
             {
 
-                printf("Trajectory length is %d\n", input.path_x.size());
+                printf("Trajectory length is %ld\n", input.path_x.size());
                 m_coefficients = m_polynomialCalculator.calculateThirdOrderPolynomial(m_localPathCut_x, m_localPathCut_y);
 
                 // feedforward based on preview point - output: target feedforward acceleration
@@ -119,8 +119,8 @@ namespace crp
             m_posDerivativeError = m_posDerivativeFilter.lowPassFilter(m_posDerivativeError, m_posDerivativeError_prev, 0.999f);
 
             double targetFbAccelerationUnfiltered = params.fbPGain * m_posErr +
-                                     params.fbDGain * m_posDerivativeError; +
-                                     params.fbIGain * m_posIntegralError;
+                                    params.fbDGain * m_posDerivativeError +
+                                    params.fbIGain * m_posIntegralError;
 
 
             // strong filtering
@@ -137,8 +137,8 @@ namespace crp
             // this method cuts a relevant length of the total trajectory, based on the complete
             // path transformed to the ego coordinate frame
             // step 1: searching for nearest point in trajectory
-            unsigned long int startIdx = -1;
-            unsigned long int stopIdx = -1;
+            unsigned long int startIdx = std::numeric_limits<unsigned long int>::max();
+            unsigned long int stopIdx  = std::numeric_limits<unsigned long int>::max();
             m_trajInvalid = false;
             double maxDistance = params.trajectory_distance; // meters
             m_localPathCut_x.clear();
@@ -156,7 +156,7 @@ namespace crp
                     break;
                 }
             }
-            if (startIdx == -1)
+            if (startIdx == std::numeric_limits<unsigned long int>::max())
             {
                 // no valid point was found
                 m_trajInvalid = true;
@@ -174,7 +174,7 @@ namespace crp
                     }
                 }
             }
-            if (stopIdx == -1)
+            if (stopIdx == std::numeric_limits<unsigned long int>::max())
             {
                 m_trajInvalid = true;
             }

crp_APL/controllers/ctrl_vehicle_control_lat_compensatory/src/ctrl_vehicle_control_lat_compensatory.cpp

@@ -1,31 +1,29 @@
 #include <ctrl_vehicle_control_lat_compensatory/ctrl_vehicle_control_lat_compensatory.hpp>
 
 
-crp::apl::CtrlVehicleControlLatCompensatory::CtrlVehicleControlLatCompensatory() : Node("CtrlVehicleControlLatCompensatory")
+crp::apl::CtrlVehicleControlLatCompensatory::CtrlVehicleControlLatCompensatory() : Node("ctrl_vehicle_control_lat_compensatory")
 {
     m_timer_ = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&CtrlVehicleControlLatCompensatory::loop, this));  
     m_pub_cmd = this->create_publisher<autoware_control_msgs::msg::Lateral>("/control/command/control_cmdLat", 30);
 
     m_sub_traj_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>("/plan/trajectory", 10, std::bind(&CtrlVehicleControlLatCompensatory::trajCallback, this, std::placeholders::_1));
     m_sub_egoVehicle_ = this->create_subscription<crp_msgs::msg::Ego>("/ego", 10, std::bind(&CtrlVehicleControlLatCompensatory::egoVehicleCallback, this, std::placeholders::_1));
 
-        this->declare_parameter("/ctrl/ffLookAheadTime", 0.68f);
-        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/fbIntegralLimit", 3.0f);
-        this->declare_parameter("/ctrl/trajectory_distance", 50.0f);
-
-        this->declare_parameter("/ctrl/sigma_thetaFP", 0.25f);
-        this->declare_parameter("/ctrl/maxThetaFP", 0.3f);
-        this->declare_parameter("/ctrl/targetAccelerationFF_lpFilterCoeff", 0.99f);
-        this->declare_parameter("/ctrl/targetAccelerationFB_lpFilterCoeff", 0.99f);
+    this->declare_parameter("/ctrl/compensatory/ffLookAheadTime", 0.68f);
+    this->declare_parameter("/ctrl/compensatory/steeringAngleLPFilter", 0.2f);
+    this->declare_parameter("/ctrl/compensatory/fbLookAheadTime", 0.0f);
+    this->declare_parameter("/ctrl/compensatory/fbPGain", 0.8f);
+    this->declare_parameter("/ctrl/compensatory/fbDGain", 1.1f);
+    this->declare_parameter("/ctrl/compensatory/fbIGain", 0.0f);
+    this->declare_parameter("/ctrl/compensatory/fbIntegralLimit", 3.0f);
+    this->declare_parameter("/ctrl/compensatory/trajectory_distance", 50.0f);
+
+    this->declare_parameter("/ctrl/compensatory/sigma_thetaFP", 0.25f);
+    this->declare_parameter("/ctrl/compensatory/maxThetaFP", 0.3f);
+    this->declare_parameter("/ctrl/compensatory/targetAccelerationFF_lpFilterCoeff", 0.99f);
+    this->declare_parameter("/ctrl/compensatory/targetAccelerationFB_lpFilterCoeff", 0.99f);
 
-
-
-    RCLCPP_INFO(this->get_logger(), "CtrlVehicleControlLatCompensatory has been started");
+    RCLCPP_INFO(this->get_logger(), "ctrl_vehicle_control_lat_compensatory has been started");
 }
 
 void crp::apl::CtrlVehicleControlLatCompensatory::trajCallback(const autoware_planning_msgs::msg::Trajectory input_msg)
@@ -72,19 +70,19 @@ void crp::apl::CtrlVehicleControlLatCompensatory::egoVehicleCallback(const crp_m
 void crp::apl::CtrlVehicleControlLatCompensatory::loop()
 {
     // parameter assignments
-    m_params.ffLookAheadTime = this->get_parameter("/ctrl/ffLookAheadTime").as_double();
-    m_params.steeringAngleLPFilter = this->get_parameter("/ctrl/steeringAngleLPFilter").as_double();
-    m_params.fbLookAheadTime = this->get_parameter("/ctrl/fbLookAheadTime").as_double();
-    m_params.fbPGain = this->get_parameter("/ctrl/fbPGain").as_double();
-    m_params.fbDGain = this->get_parameter("/ctrl/fbDGain").as_double();
-    m_params.fbIGain = this->get_parameter("/ctrl/fbIGain").as_double();
-    m_params.fbIntegralLimit = this->get_parameter("/ctrl/fbIntegralLimit").as_double();
-    m_params.trajectory_distance = this->get_parameter("/ctrl/trajectory_distance").as_double();
-
-    m_params.sigma_thetaFP = this->get_parameter("/ctrl/sigma_thetaFP").as_double();
-    m_params.maxThetaFP = this->get_parameter("/ctrl/maxThetaFP").as_double();
-    m_params.targetAccelerationFF_lpFilterCoeff = this->get_parameter("/ctrl/targetAccelerationFF_lpFilterCoeff").as_double();
-    m_params.targetAccelerationFB_lpFilterCoeff = this->get_parameter("/ctrl/targetAccelerationFB_lpFilterCoeff").as_double();
+    m_params.ffLookAheadTime       = this->get_parameter("/ctrl/compensatory/ffLookAheadTime").as_double();
+    m_params.steeringAngleLPFilter = this->get_parameter("/ctrl/compensatory/steeringAngleLPFilter").as_double();
+    m_params.fbLookAheadTime       = this->get_parameter("/ctrl/compensatory/fbLookAheadTime").as_double();
+    m_params.fbPGain               = this->get_parameter("/ctrl/compensatory/fbPGain").as_double();
+    m_params.fbDGain               = this->get_parameter("/ctrl/compensatory/fbDGain").as_double();
+    m_params.fbIGain               = this->get_parameter("/ctrl/compensatory/fbIGain").as_double();
+    m_params.fbIntegralLimit       = this->get_parameter("/ctrl/compensatory/fbIntegralLimit").as_double();
+    m_params.trajectory_distance   = this->get_parameter("/ctrl/compensatory/trajectory_distance").as_double();
+
+    m_params.sigma_thetaFP                      = this->get_parameter("/ctrl/compensatory/sigma_thetaFP").as_double();
+    m_params.maxThetaFP                         = this->get_parameter("/ctrl/compensatory/maxThetaFP").as_double();
+    m_params.targetAccelerationFF_lpFilterCoeff = this->get_parameter("/ctrl/compensatory/targetAccelerationFF_lpFilterCoeff").as_double();
+    m_params.targetAccelerationFB_lpFilterCoeff = this->get_parameter("/ctrl/compensatory/targetAccelerationFB_lpFilterCoeff").as_double();
 
     m_compensatoryModel.run(m_input, m_output, m_params);
 

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/CMakeLists.txt

@@ -20,11 +20,13 @@ add_executable(ctrl_vehicle_control_lat_pure_p src/ctrl_vehicle_control_lat_pure
 
 ament_target_dependencies(ctrl_vehicle_control_lat_pure_p  rclcpp autoware_control_msgs autoware_planning_msgs crp_msgs)
 
-install(DIRECTORY launch
+install(DIRECTORY
+  config
+  launch
   DESTINATION share/${PROJECT_NAME})
 
 install(TARGETS
-ctrl_vehicle_control_lat_pure_p
+  ctrl_vehicle_control_lat_pure_p
   DESTINATION lib/${PROJECT_NAME})
 
 ament_package()

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/config/ctrlPurePParams.yaml

@@ -0,0 +1,4 @@
+ctrl_vehicle_control_lat_pure_pursuit:
+  ros__parameters:
+    /ctrl/pure_p/lookahead_time: 1.0
+    /ctrl/pure_p/wheelbase:      2.7

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/launch/ctrl_vehicle_control_lat_pure_p.launch.py

@@ -1,16 +1,36 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+from os.path import join
+
 
 def generate_launch_description():
+    # ARGUMENTS
+
+    ctrlPurePConfigFileArg = DeclareLaunchArgument(
+        'ctrlPurePConfigFile',
+        default_value=join(
+            get_package_share_directory('ctrl_vehicle_control_lat_pure_p'),
+            'config',
+            'ctrlPurePParams.yaml'
+        ),
+        description='Path to the pure pursuit control configuration file'
+    )
+
+    # NODES
+
     ctrl_vehicle_control_lat_pure_p = Node(
         package="ctrl_vehicle_control_lat_pure_p",
         executable="ctrl_vehicle_control_lat_pure_p",
         parameters=[
-            {"/ctrl/lookahead_time": 1.0},
-            {"/ctrl/wheelbase": 2.7},
+            LaunchConfiguration('ctrlPurePConfigFile'),
         ]
     )
 
     return LaunchDescription([
+        ctrlPurePConfigFileArg,
+
         ctrl_vehicle_control_lat_pure_p
     ])

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/src/ctrl_vehicle_control_lat_pure_p.cpp

@@ -1,18 +1,18 @@
 #include <ctrl_vehicle_control_lat_pure_p/ctrl_vehicle_control_lat_pure_p.hpp>
 
 
-crp::apl::CtrlVehicleControlLatPureP::CtrlVehicleControlLatPureP() : Node("CtrlVehicleControlLatPurePursuit")
+crp::apl::CtrlVehicleControlLatPureP::CtrlVehicleControlLatPureP() : Node("ctrl_vehicle_control_lat_pure_pursuit")
 {
     m_timer_ = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&CtrlVehicleControlLatPureP::loop, this));  
     m_pub_cmd = this->create_publisher<autoware_control_msgs::msg::Lateral>("/control/command/control_cmdLat", 30);
 
     m_sub_traj_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>("/plan/trajectory", 10, std::bind(&CtrlVehicleControlLatPureP::trajCallback, this, std::placeholders::_1));
     m_sub_egoVehicle_ = this->create_subscription<crp_msgs::msg::Ego>("/ego", 10, std::bind(&CtrlVehicleControlLatPureP::egoVehicleCallback, this, std::placeholders::_1));
 
-    this->declare_parameter("/ctrl/lookahead_time", 1.0f);
-    this->declare_parameter("/ctrl/wheelbase", 2.7f);
+    this->declare_parameter("/ctrl/pure_p/lookahead_time", 1.0f);
+    this->declare_parameter("/ctrl/pure_p/wheelbase", 2.7f);
 
-    RCLCPP_INFO(this->get_logger(), "CtrlVehicleControlLatPurePursuit has been started");
+    RCLCPP_INFO(this->get_logger(), "ctrl_vehicle_control_lat_pure_pursuit has been started");
 }
 
 void crp::apl::CtrlVehicleControlLatPureP::trajCallback(const autoware_planning_msgs::msg::Trajectory input_msg)
@@ -49,7 +49,7 @@ void crp::apl::CtrlVehicleControlLatPureP::pure_p_control()
     float currentDistance = 0.0f;
     int target_index = 0;
 
-    for (int i = 0; i < m_input.m_path_x.size()-1; i++)
+    for (unsigned int i = 0; i < m_input.m_path_x.size()-1; i++)
     {
         currentDistance += sqrt(pow(m_input.m_path_x.at(i+1) - m_input.m_path_x.at(i), 2) + pow(m_input.m_path_y.at(i+1) - m_input.m_path_y.at(i), 2));
         if (currentDistance >= distance_to_index)
@@ -69,8 +69,8 @@ void crp::apl::CtrlVehicleControlLatPureP::pure_p_control()
 void crp::apl::CtrlVehicleControlLatPureP::loop()
 {
     // parameter assignments
-    m_params.lookaheadTime = this->get_parameter("/ctrl/lookahead_time").as_double();
-    m_params.wheelbase = this->get_parameter("/ctrl/wheelbase").as_double();
+    m_params.lookaheadTime = this->get_parameter("/ctrl/pure_p/lookahead_time").as_double();
+    m_params.wheelbase = this->get_parameter("/ctrl/pure_p/wheelbase").as_double();
 
     // control algorithm
     if(m_input.m_path_x.size() > 0 && m_input.m_path_y.size() > 0)

crp_APL/controllers/ctrl_vehicle_control_lat_stanley/CMakeLists.txt

@@ -20,7 +20,9 @@ add_executable(ctrl_vehicle_control_lat_stanley src/ctrl_vehicle_control_lat_sta
 
 ament_target_dependencies(ctrl_vehicle_control_lat_stanley  rclcpp autoware_control_msgs autoware_planning_msgs crp_msgs)
 
-install(DIRECTORY launch
+install(DIRECTORY
+  config
+  launch
   DESTINATION share/${PROJECT_NAME})
 
 install(TARGETS

crp_APL/controllers/ctrl_vehicle_control_lat_stanley/config/ctrlStanleyParams.yaml

@@ -0,0 +1,4 @@
+ctrl_vehicle_control_lat_stanley:
+  ros__parameters:
+    /ctrl/stanley/k_gain:    1.5
+    /ctrl/stanley/wheelbase: 2.7

crp_APL/controllers/ctrl_vehicle_control_lat_stanley/launch/ctrl_vehicle_control_lat_stanley.launch.py

@@ -1,16 +1,36 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+from os.path import join
+
 
 def generate_launch_description():
+    # ARGUMENTS
+
+    ctrlStanleyConfigFileArg = DeclareLaunchArgument(
+        'ctrlStanleyConfigFile',
+        default_value=join(
+            get_package_share_directory('ctrl_vehicle_control_lat_stanley'),
+            'config',
+            'ctrlStanleyParams.yaml'
+        ),
+        description='Path to the stanley control configuration file'
+    )
+
+    # NODES
+
     ctrl_vehicle_control_lat_stanley = Node(
         package="ctrl_vehicle_control_lat_stanley",
         executable="ctrl_vehicle_control_lat_stanley",
         parameters=[
-            {"/ctrl/k_gain": 1.5},
-            {"/ctrl/wheelbase": 2.7},
+            LaunchConfiguration('ctrlStanleyConfigFile'),
         ]
     )
 
     return LaunchDescription([
+        ctrlStanleyConfigFileArg,
+
         ctrl_vehicle_control_lat_stanley
     ])