diff --git a/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml b/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml index b1279b50ef04..8941b92b4e78 100644 --- a/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml +++ b/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml @@ -9,3 +9,4 @@ left_overhang: 0.1 # between left wheel center and vehicle left right_overhang: 0.1 # between right wheel center and vehicle right vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml index 12b6efa79d1c..ef594927dac4 100644 --- a/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml +++ b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml @@ -10,3 +10,4 @@ width_m: 2.0 front_overhang_m: 0.5 rear_overhang_m: 0.5 + max_steer_angle: 0.70 # [rad] diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index d983aeed4d8c..fea6cd16db9d 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -190,6 +190,7 @@ void declareVehicleInfoParams(rclcpp::NodeOptions & node_options) node_options.append_parameter_override("left_overhang", 0.5); node_options.append_parameter_override("right_overhang", 0.5); node_options.append_parameter_override("vehicle_height", 1.5); + node_options.append_parameter_override("max_steer_angle", 0.7); } // Send a control command and run the simulation. diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml index b1279b50ef04..8941b92b4e78 100644 --- a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml +++ b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml @@ -9,3 +9,4 @@ left_overhang: 0.1 # between left wheel center and vehicle left right_overhang: 0.1 # between right wheel center and vehicle right vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp index e7ba44605188..8df6d4cfe04c 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp +++ b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp @@ -31,6 +31,7 @@ struct VehicleInfo double left_overhang_m; double right_overhang_m; double vehicle_height_m; + double max_steer_angle_m; // Derived parameters, i.e. calculated from base parameters // The offset values are relative to the base frame origin, which is located @@ -49,7 +50,8 @@ struct VehicleInfo inline VehicleInfo createVehicleInfo( const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m, const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m, - const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m) + const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m, + const double max_steer_angle_m) { // Calculate derived parameters const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m; @@ -72,6 +74,7 @@ inline VehicleInfo createVehicleInfo( left_overhang_m, right_overhang_m, vehicle_height_m, + max_steer_angle_m, // Derived parameters vehicle_length_m_, vehicle_width_m_, diff --git a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp b/vehicle/vehicle_info_util/src/vehicle_info_util.cpp index 6cc2d2f27e9c..c295155727e6 100644 --- a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp +++ b/vehicle/vehicle_info_util/src/vehicle_info_util.cpp @@ -49,6 +49,7 @@ VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node) vehicle_info_.left_overhang_m = getParameter(node, "left_overhang"); vehicle_info_.right_overhang_m = getParameter(node, "right_overhang"); vehicle_info_.vehicle_height_m = getParameter(node, "vehicle_height"); + vehicle_info_.max_steer_angle_m = getParameter(node, "max_steer_angle"); } VehicleInfo VehicleInfoUtil::getVehicleInfo() @@ -56,7 +57,8 @@ VehicleInfo VehicleInfoUtil::getVehicleInfo() return createVehicleInfo( vehicle_info_.wheel_radius_m, vehicle_info_.wheel_width_m, vehicle_info_.wheel_base_m, vehicle_info_.wheel_tread_m, vehicle_info_.front_overhang_m, vehicle_info_.rear_overhang_m, - vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m); + vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m, + vehicle_info_.max_steer_angle_m); } } // namespace vehicle_info_util