Merge pull request #95 from jkk-research/ZZtest_01_16

Z ztest 01 16

crp_APL/controllers/ctrl_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},
         ]
     )
 

crp_APL/controllers/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 +

crp_APL/controllers/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);

crp_APL/controllers/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},
         ]
     )

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
             }]
         )
     ])

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;

crp_CIL/novatel_gps_wrapper/src/novatelTopicConverter.cpp

@@ -14,20 +14,7 @@ void crp::vil::NovatelTopicConverter::currentPoseCallback(const geometry_msgs::m
     geometry_msgs::msg::PoseWithCovarianceStamped poseWithCovariance;
     poseWithCovariance.header = msg->header;
     poseWithCovariance.pose.pose = msg->pose;
-
-    tf2::Quaternion q_orig;
-    tf2::convert(msg->pose.orientation, q_orig);
-
-    tf2::Matrix3x3 m(q_orig);
-    double roll, pitch, yaw;
-    m.getRPY(roll, pitch, yaw);
-
-    yaw += M_PI / 2.0;
-
-    tf2::Quaternion q_rotated;
-    q_rotated.setRPY(roll, pitch, yaw);
-
-    poseWithCovariance.pose.pose.orientation = tf2::toMsg(q_rotated);
+    poseWithCovariance.pose.pose.orientation = msg->pose.orientation;
 
     m_pub_currentPoseWithCovariance_->publish(poseWithCovariance);
 }

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/Downloads/ZalaZone_Handling.osm",
+        default_value="/home/dev/lanelet2_maps/auto_mapper/lightweight_lanelet.osm",
         description='Path to the lanelet2 map file'
     )
 

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;
 

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);
 }
 
 

launch/crp_launcher/launch/core_lqr.launch.py

@@ -0,0 +1,102 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import LaunchConfigurationEquals
+from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+from os.path import join
+
+
+def generate_launch_description():
+    # NODES
+
+    environmental_fusion = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('prcp_situation_analysis'),
+                'launch',
+                'environmental_fusion.launch.py')
+        )
+    )
+
+    behavior_planning = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_behavior_planning'),
+                'launch',
+                'behavior_planning.launch.py'
+            )
+        )
+    )
+
+    motion_planning = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_motion_planning'),
+                'launch',
+                'motion_planning.launch.py'
+            )
+        )
+    )
+
+    planner_lat_lane_follow_ldm = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_lat_lane_follow_ldm'),
+                'launch',
+                'plan_lat_lane_follow_ldm.launch.py'
+            )
+        )
+    )
+
+    planner_lon_intelligent_speed_adjust = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_lon_intelligent_speed_adjust'),
+                'launch',
+                'plan_lon_intelligent_speed_adjust.launch.py'
+            )
+        )
+    )
+
+    vehicle_control = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control'),
+                'launch',
+                'ctrl_vehicle_control.launch.py')
+        )
+    )
+
+    vehicle_control_lat = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control_lqr'),
+                'launch',
+                'lat_lon_control_lqr.launch.py')
+        )
+    )
+
+    # vehicle_control_long = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         join(
+    #             get_package_share_directory('ctrl_vehicle_control_long'),
+    #             'launch',
+    #             'ctrl_vehicle_control_long.launch.py')
+    #     )
+    # )
+
+    return LaunchDescription([
+        # nodes
+        environmental_fusion,
+        behavior_planning,
+        planner_lat_lane_follow_ldm,
+        planner_lon_intelligent_speed_adjust,
+        motion_planning,
+        vehicle_control,
+        vehicle_control_lat,
+        # vehicle_control_long,
+
+
+    ])

launch/crp_launcher/launch/core_pure_p.launch.py

@@ -0,0 +1,102 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import LaunchConfigurationEquals
+from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+from os.path import join
+
+
+def generate_launch_description():
+    # NODES
+
+    environmental_fusion = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('prcp_situation_analysis'),
+                'launch',
+                'environmental_fusion.launch.py')
+        )
+    )
+
+    behavior_planning = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_behavior_planning'),
+                'launch',
+                'behavior_planning.launch.py'
+            )
+        )
+    )
+
+    motion_planning = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_motion_planning'),
+                'launch',
+                'motion_planning.launch.py'
+            )
+        )
+    )
+
+    planner_lat_lane_follow_ldm = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_lat_lane_follow_ldm'),
+                'launch',
+                'plan_lat_lane_follow_ldm.launch.py'
+            )
+        )
+    )
+
+    planner_lon_intelligent_speed_adjust = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('plan_lon_intelligent_speed_adjust'),
+                'launch',
+                'plan_lon_intelligent_speed_adjust.launch.py'
+            )
+        )
+    )
+
+    vehicle_control = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control'),
+                'launch',
+                'ctrl_vehicle_control.launch.py')
+        )
+    )
+
+    vehicle_control_lat = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control_lat_pure_p'),
+                'launch',
+                'ctrl_vehicle_control_lat_pure_p.launch.py')
+        )
+    )
+
+    vehicle_control_long = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            join(
+                get_package_share_directory('ctrl_vehicle_control_long'),
+                'launch',
+                'ctrl_vehicle_control_long.launch.py')
+        )
+    )
+
+    return LaunchDescription([
+        # nodes
+        environmental_fusion,
+        behavior_planning,
+        planner_lat_lane_follow_ldm,
+        planner_lon_intelligent_speed_adjust,
+        motion_planning,
+        vehicle_control,
+        vehicle_control_lat,
+        vehicle_control_long,
+
+
+    ])