diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 9c64b64645c6f..b241c8c45bb3e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -248,6 +248,7 @@ vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4. vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp -vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp +vehicle/autoware_vehicle_info_utils/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp ### Copied from .github/CODEOWNERS-manual ### diff --git a/common/global_parameter_loader/launch/global_params.launch.py b/common/global_parameter_loader/launch/global_params.launch.py index 06f807aaa4609..8f9092b10fa37 100644 --- a/common/global_parameter_loader/launch/global_params.launch.py +++ b/common/global_parameter_loader/launch/global_params.launch.py @@ -33,7 +33,7 @@ def launch_setup(context, *args, **kwargs): load_vehicle_info = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("vehicle_info_util"), "/launch/vehicle_info.launch.py"] + [FindPackageShare("autoware_vehicle_info_utils"), "/launch/vehicle_info.launch.py"] ), launch_arguments={ "vehicle_info_param_file": [vehicle_description_pkg, "/config/vehicle_info.param.yaml"] diff --git a/common/global_parameter_loader/package.xml b/common/global_parameter_loader/package.xml index 4c2568b9aec98..472ef0c063430 100644 --- a/common/global_parameter_loader/package.xml +++ b/common/global_parameter_loader/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils ament_lint_auto autoware_lint_common diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index f2482b2d45a3b..1c1c4ad6d8ba1 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils geometry_msgs libqt5-core libqt5-gui @@ -20,7 +21,6 @@ rviz_common rviz_default_plugins tf2_ros - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index c4233bd6305e3..cd32df498bfd4 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -172,7 +172,7 @@ void PoseHistoryFootprint::updateFootprint() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_THROTTLE( diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp index d957054bd479e..44625ad7deb41 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp @@ -15,8 +15,8 @@ #ifndef POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ #define POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ +#include #include -#include #include @@ -39,8 +39,8 @@ class BoolProperty; namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; class PoseHistoryFootprint : public rviz_common::MessageFilterDisplay diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index ed79e122c0a01..9cb710c239029 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -15,6 +15,7 @@ #ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ #define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include @@ -97,8 +97,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; template class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay { diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 5d755d212c859..95edcb7a96c56 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_planning_msgs + autoware_vehicle_info_utils libqt5-core libqt5-gui libqt5-widgets @@ -25,7 +26,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 4a297b47bcfcc..342e72d66c5f9 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -31,7 +31,7 @@ void AutowarePathWithLaneIdDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -107,7 +107,7 @@ void AutowarePathDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -124,7 +124,7 @@ void AutowareTrajectoryDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( diff --git a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp index b87f421de7c8c..3e72d72cb9946 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ #define AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#include #include #include #include #include #include #include -#include #include #include @@ -59,11 +59,11 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; +using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 221fc2c000826..0404eec7ca308 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -17,6 +17,7 @@ autoware_control_msgs autoware_planning_msgs autoware_system_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs @@ -34,7 +35,6 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 3ec8cc68df9b1..d01855bc8ab5d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -60,7 +60,7 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d createPolygon( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) { Polygon2d polygon; @@ -102,7 +102,7 @@ Polygon2d createPolygon( AEB::AEB(const rclcpp::NodeOptions & node_options) : Node("AEB", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), collision_data_keeper_(this->get_clock()) { // Publisher diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 3e62d69f6ebb6..673d9d7896745 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -19,6 +19,7 @@ autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diagnostic_updater @@ -35,7 +36,6 @@ tier4_autoware_utils tier4_debug_msgs trajectory_follower_base - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 3d66078eed1f2..cf8a4ed5a251b 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -19,10 +19,10 @@ #include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -69,7 +69,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps"); // [rad/s] /* mpc parameters */ - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad; diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp index 7b9d999a3decb..08a6a51d3098b 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -20,6 +20,7 @@ #include "autoware_pid_longitudinal_controller/lowpass_filter.hpp" #include "autoware_pid_longitudinal_controller/pid.hpp" #include "autoware_pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -27,7 +28,6 @@ #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/ros/marker_helper.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index 50ae7ccaf2e55..66f6a6dfcda8b 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diagnostic_updater @@ -36,7 +37,6 @@ tier4_autoware_utils tier4_debug_msgs trajectory_follower_base - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index e1dd414b2446d..4cca525f6ebf3 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -38,9 +38,11 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters timer m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); - m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m; - m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m; - m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m; + m_wheel_base = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().wheel_base_m; + m_vehicle_width = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().vehicle_width_m; + m_front_overhang = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().front_overhang_m; // parameters for delay compensation m_delay_compensation_time = node.declare_parameter("delay_compensation_time"); // [s] diff --git a/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index c9e83bf9878b8..c9df4e67c2647 100644 --- a/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml index da0e885cc564a..f67d01a5593ae 100644 --- a/control/autoware_vehicle_cmd_gate/package.xml +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -17,6 +17,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -33,7 +34,6 @@ tier4_external_api_msgs tier4_system_msgs tier4_vehicle_msgs - vehicle_info_util visualization_msgs rosidl_default_runtime diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index e8d35beb94b1f..bb50dcb586e83 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -170,7 +170,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("filter_activated_velocity_threshold"); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; @@ -276,7 +276,7 @@ rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( parameters, "filter_activated_velocity_threshold", filter_activated_velocity_threshold_); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p = filter_.getParam(); p.wheel_base = vehicle_info.wheel_base_m; diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 22613de5d1f07..06dad536d842d 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -21,11 +21,11 @@ #include "vehicle_cmd_filter.hpp" #include +#include #include #include #include #include -#include #include #include diff --git a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 6f148fe6c8140..46130fc8f9941 100644 --- a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -371,7 +371,7 @@ std::shared_ptr generateNode() const auto vehicle_cmd_gate_dir = ament_index_cpp::get_package_share_directory("autoware_vehicle_cmd_gate"); const auto vehicle_info_util_dir = - ament_index_cpp::get_package_share_directory("vehicle_info_util"); + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); node_options.arguments( {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml", diff --git a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml index be2b30bdf1bf9..f2997f0e10b83 100644 --- a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml +++ b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index a26f4164a177b..b18fb8c98ccdc 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -26,6 +26,7 @@ autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs libboost-dev @@ -41,7 +42,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - vehicle_info_util builtin_interfaces global_parameter_loader rosidl_default_runtime diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 693194e8b70a6..47ce3ce07e83c 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -17,7 +17,7 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" -#include +#include #include #include @@ -32,7 +32,7 @@ using control_performance_analysis::msg::ErrorStamped; namespace control_performance_analysis { -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfoUtils; ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( const rclcpp::NodeOptions & node_options) @@ -41,7 +41,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( using std::placeholders::_1; // Implement Reading Global and Local Variables. - const auto & vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto & vehicle_info = VehicleInfoUtils(*this).getVehicleInfo(); param_.wheelbase_ = vehicle_info.wheel_base_m; // Node Parameters. diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index e048ef03bf11a..56125cc40b74e 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -15,9 +15,9 @@ #ifndef CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ #define CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "control_validator/debug_marker.hpp" #include "control_validator/msg/control_validator_status.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -83,7 +83,7 @@ class ControlValidator : public rclcpp::Node ControlValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool isAllValid(const ControlValidatorStatus & status); diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index 2f4400d6ebc57..93779d28d6218 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -21,6 +21,7 @@ rosidl_default_generators autoware_planning_msgs + autoware_vehicle_info_utils diagnostic_updater geometry_msgs motion_utils @@ -28,7 +29,6 @@ rclcpp rclcpp_components tier4_autoware_utils - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 5282e31fef898..be103647f1b36 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -61,7 +61,7 @@ void ControlValidator::setupParameters() } try { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); } catch (...) { RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); vehicle_info_.front_overhang_m = 0.5; diff --git a/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp index d90dbc9474fd8..e29623bf69891 100644 --- a/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ #define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#include #include #include #include -#include #include #include @@ -103,17 +103,17 @@ class LaneDepartureChecker public: Output update(const Input & input); - void setParam(const Param & param, const vehicle_info_util::VehicleInfo vehicle_info) + void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { param_ = param; - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } void setParam(const Param & param) { param_ = param; } - void setVehicleInfo(const vehicle_info_util::VehicleInfo vehicle_info) + void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } bool checkPathWillLeaveLane( @@ -137,7 +137,7 @@ class LaneDepartureChecker private: Param param_; - std::shared_ptr vehicle_info_ptr_; + std::shared_ptr vehicle_info_ptr_; static PoseDeviation calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, diff --git a/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp index 917317361d702..c85e37212b16f 100644 --- a/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp @@ -29,8 +29,8 @@ #define AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #include +#include #include -#include #include diff --git a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml index 7ea2e8b2de60a..b9f820f66690b 100644 --- a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml +++ b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 466dc7351cd9e..7a08cd2907120 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -15,6 +15,7 @@ autoware_map_msgs autoware_planning_msgs + autoware_vehicle_info_utils boost diagnostic_updater eigen @@ -30,7 +31,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 7956410fdf898..b25b3fd843f4b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -145,7 +145,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o declare_parameter>("boundary_types_to_detect"); // Vehicle Info - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_length_m_ = vehicle_info.vehicle_length_m; // Core Parameter diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index a65d818056bd7..efaefd765e793 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -15,8 +15,8 @@ #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#include #include -#include #include #include @@ -75,7 +75,7 @@ class ObstacleCollisionChecker private: Param param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; //! This function assumes the input trajectory is sampled dense enough static autoware_planning_msgs::msg::Trajectory resampleTrajectory( @@ -86,7 +86,7 @@ class ObstacleCollisionChecker static std::vector createVehicleFootprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 078993d064064..f82384534e040 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index ef7560755a122..795302abe75f4 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_planning_msgs + autoware_vehicle_info_utils boost diagnostic_updater eigen @@ -33,7 +34,6 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 6887be4cedd77..a844fe6a50cbf 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -83,7 +83,7 @@ double calcBrakingDistance( namespace obstacle_collision_checker { ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) -: vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) +: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { } @@ -188,7 +188,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( std::vector ObstacleCollisionChecker::createVehicleFootprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // Create vehicle footprint in base_link coordinate const auto local_vehicle_footprint = vehicle_info.createFootprint(param.footprint_margin); diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index cfde4ee849728..402123c81d361 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -14,10 +14,10 @@ #include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" +#include #include #include #include -#include #include #include diff --git a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml index 569d743ba2c4f..c6f03db0fcacc 100644 --- a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml +++ b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index 81517675281c3..40232469fdb6a 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -13,6 +13,7 @@ rosidl_default_generators autoware_control_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -25,7 +26,6 @@ tier4_control_msgs tier4_system_msgs tier4_vehicle_msgs - vehicle_info_util ament_cmake_gtest ament_lint_auto diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 635ead2430677..82270875b2e17 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -33,7 +33,7 @@ using tier4_autoware_utils::calcYawDeviation; AutonomousMode::AutonomousMode(rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); sub_control_cmd_ = node->create_subscription( "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index e5abd4285ad5f..54434690f2147 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -18,8 +18,8 @@ #include "data.hpp" #include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" +#include #include -#include #include #include @@ -83,7 +83,7 @@ class AutonomousMode : public ModeChangeBase Control trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; DebugInfo debug_info_; std::shared_ptr stable_start_time_; // Reset every transition start. diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index 8410e7c78f723..2a7a552c1973d 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,6 +15,7 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include #include #include #include @@ -23,7 +24,6 @@ #include #include #include -#include #include #include @@ -120,7 +120,7 @@ class CollisionChecker // Variables std::shared_ptr debug_ptr_; rclcpp::Node * node_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector predicted_object_history_{}; }; } // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 23696887c7051..6c4832a0dac3e 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -15,6 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ #define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#include +#include #include #include #include @@ -24,8 +26,6 @@ #include #include #include -#include -#include #include #include @@ -116,7 +116,7 @@ class PredictedPathCheckerNode : public rclcpp::Node // Variables State current_state_{State::DRIVE}; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool is_calling_set_stop_{false}; bool is_stopped_by_node_{false}; diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 957800ad04305..d9299f09dfb6b 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include #include #include #include -#include #include #include @@ -39,6 +39,7 @@ namespace utils { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -48,7 +49,6 @@ using geometry_msgs::msg::TransformStamped; using std_msgs::msg::Header; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; using PointArray = std::vector; using TrajectoryPoints = std::vector; @@ -57,7 +57,7 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width); TrajectoryPoint calcInterpolatedPoint( const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, @@ -65,7 +65,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, - const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + const double stop_margin, autoware::vehicle_info_utils::VehicleInfo & vehicle_info); bool isInBrakeDistance( const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml index a35c11b80c1f7..6af1372a5bb4a 100755 --- a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index bca65302dba55..911344c578414 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -14,6 +14,7 @@ autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils boost component_interface_specs component_interface_utils @@ -34,7 +35,6 @@ tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp index 5e8d96805b832..7d392571546b4 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -27,7 +27,7 @@ CollisionChecker::CollisionChecker( rclcpp::Node * node, std::shared_ptr debug_ptr) : debug_ptr_(std::move(debug_ptr)), node_(node), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo()) + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo()) { } diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index 28ea21f639a0e..38717a1849ad9 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -38,7 +38,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.init_cli(cli_set_stop_, group_cli_); adaptor.init_sub(sub_stop_state_, this, &PredictedPathCheckerNode::onIsStopped); - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Node Parameter node_param_.update_rate = declare_parameter("update_rate", 10.0); diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index f1c76ce6eef8f..a9c3e2b24f9a5 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -29,7 +29,7 @@ using tier4_autoware_utils::getRPY; // Utils Functions Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) { Polygon2d polygon; @@ -146,7 +146,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, - vehicle_info_util::VehicleInfo & vehicle_info) + autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // It returns the stop point and segment of the point on trajectory. diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 0747766af54c8..32744d744f95f 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -16,6 +16,7 @@ autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils boost geometry_msgs libboost-dev @@ -32,7 +33,6 @@ tier4_autoware_utils tier4_debug_msgs trajectory_follower_base - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index a8232cce5d08d..eddcdb5e362df 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -34,7 +34,7 @@ #include "pure_pursuit/util/planning_utils.hpp" #include "pure_pursuit/util/tf_utils.hpp" -#include +#include #include #include @@ -65,7 +65,7 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) pure_pursuit_ = std::make_unique(); // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); param_.wheel_base = vehicle_info.wheel_base_m; param_.max_steering_angle = vehicle_info.max_steer_angle_rad; diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp index a4b491930df1e..d629c8f549f52 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -34,7 +34,7 @@ #include "pure_pursuit/util/planning_utils.hpp" #include "pure_pursuit/util/tf_utils.hpp" -#include +#include #include #include @@ -59,7 +59,7 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) pure_pursuit_ = std::make_unique(); // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); param_.wheel_base = vehicle_info.wheel_base_m; // Node Parameters diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index 6f2e9c3e8ebc2..b36b4669be04e 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -22,6 +22,7 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diagnostic_updater @@ -37,7 +38,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 8e9752ba19f66..b342aace69bde 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -15,6 +15,7 @@ #ifndef TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ #define TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" @@ -24,7 +25,6 @@ #include "trajectory_follower_base/lateral_controller_base.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "trajectory_follower_node/visibility_control.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 92b99e690931f..c57a07155128a 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -24,6 +24,7 @@ autoware_mpc_lateral_controller autoware_pid_longitudinal_controller autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs motion_utils pure_pursuit @@ -31,7 +32,6 @@ rclcpp_components tier4_autoware_utils trajectory_follower_base - vehicle_info_util visualization_msgs ros2launch diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index be335a2d78fa3..b05bb5e1d8ad3 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -15,7 +15,7 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ -#include +#include #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include @@ -45,10 +45,10 @@ inline int64_t bitShift(int64_t original_id) void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 3f855e2f1f887..d64d3a5ec4957 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_perception_msgs + autoware_vehicle_info_utils diagnostic_msgs eigen geometry_msgs @@ -32,7 +33,6 @@ tf2 tf2_ros tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index 523e11883e755..eddeadc331101 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -39,7 +39,7 @@ using visualization_msgs::msg::Marker; void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double half_width = vehicle_info.vehicle_width_m / 2.0; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; @@ -57,7 +57,7 @@ void addFootprintMarker( } MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); diff --git a/launch/tier4_sensing_launch/launch/sensing.launch.xml b/launch/tier4_sensing_launch/launch/sensing.launch.xml index 391a1f8bad641..9981c99a8f3fc 100644 --- a/launch/tier4_sensing_launch/launch/sensing.launch.xml +++ b/launch/tier4_sensing_launch/launch/sensing.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_sensing_launch/package.xml b/launch/tier4_sensing_launch/package.xml index a1f10ee743db5..410f685285b7d 100644 --- a/launch/tier4_sensing_launch/package.xml +++ b/launch/tier4_sensing_launch/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils ament_lint_auto autoware_lint_common diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index d97bbaa2118e5..314c9cfb97a75 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -18,7 +18,7 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/transform_info.hpp" -#include +#include #include @@ -43,7 +43,7 @@ class ScanGroundFilterTest; namespace ground_segmentation { -using vehicle_info_util::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfo; class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter { diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index d3d856925b9db..df8290997315e 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -85,7 +85,7 @@ def add_launch_arg(name: str, default_value=None): return DeclareLaunchArgument(name, default_value=default_value) default_vehicle_info_param = os.path.join( - get_package_share_directory("vehicle_info_util"), "config/vehicle_info.param.yaml" + get_package_share_directory("autoware_vehicle_info_utils"), "config/vehicle_info.param.yaml" ) vehicle_info_param = DeclareLaunchArgument( diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml index 2a9e4983ecb56..1146138211211 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/ground_segmentation/package.xml b/perception/ground_segmentation/package.xml index 05d5d18baf29e..1b807b393b739 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/ground_segmentation/package.xml @@ -19,6 +19,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils libopencv-dev pcl_conversions pcl_ros @@ -31,7 +32,6 @@ tf2_eigen tf2_ros tf2_sensor_msgs - vehicle_info_util yaml-cpp ament_lint_auto diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index a63d218df62de..e638baba12521 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -14,10 +14,10 @@ #include "ground_segmentation/scan_ground_filter_nodelet.hpp" +#include #include #include #include -#include #include #include @@ -25,12 +25,12 @@ namespace ground_segmentation { +using autoware::vehicle_info_utils::VehicleInfoUtils; using pointcloud_preprocessor::get_param; using tier4_autoware_utils::calcDistance3d; using tier4_autoware_utils::deg2rad; using tier4_autoware_utils::normalizeDegree; using tier4_autoware_utils::normalizeRadian; -using vehicle_info_util::VehicleInfoUtil; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("ScanGroundFilter", options) @@ -59,7 +59,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); use_lowest_point_ = declare_parameter("use_lowest_point"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); - vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp index 4abbfd419c43b..04873d9ebcfab 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp @@ -18,7 +18,7 @@ #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include @@ -60,10 +60,10 @@ inline int64_t bitShift(int64_t original_id) void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( @@ -107,8 +107,9 @@ MarkerArray createDrivableLanesMarkerArray( const std::vector & drivable_lanes, std::string && ns); MarkerArray createPredictedPathMarkerArray( - const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, - std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + const PredictedPath & ego_predicted_path, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, std::string && ns, + const int32_t & id, const float & r, const float & g, const float & b); MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp index 21d4906380b59..2f14e227d05ef 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ -#include +#include struct ModuleConfigParameters { @@ -66,7 +66,7 @@ struct BehaviorPathPlannerParameters double ego_nearest_yaw_threshold; // vehicle info - vehicle_info_util::VehicleInfo vehicle_info; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; double wheel_base; double front_overhang; double rear_overhang; diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp index 4dda036fecdb7..d658d29710704 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp @@ -237,7 +237,7 @@ class TurnSignalDecider inline bool straddleRoadBound( const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, - const vehicle_info_util::VehicleInfo & vehicle_info) const + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) const { using boost::geometry::intersects; using tier4_autoware_utils::pose2transform; diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 32613948814ae..35da7e4e24382 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#include #include -#include #include #include @@ -83,7 +83,7 @@ struct DrivableAreaExpansionParameters bool avoid_dynamic_objects{}; bool print_runtime{}; std::vector avoid_linestring_types{}; - vehicle_info_util::VehicleInfo vehicle_info; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -121,7 +121,7 @@ struct DrivableAreaExpansionParameters avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); - vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); } }; diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index d0693a271379e..018e1c87e2302 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -31,6 +31,7 @@ namespace behavior_path_planner::utils::path_safety_checker { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; @@ -40,7 +41,6 @@ using geometry_msgs::msg::Twist; using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose, @@ -48,13 +48,13 @@ bool isTargetObjectOncoming( bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); Polygon2d createExtendedPolygon( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( diff --git a/planning/autoware_behavior_path_planner_common/package.xml b/planning/autoware_behavior_path_planner_common/package.xml index 03391063b14fc..e9b33f3e71d6f 100644 --- a/planning/autoware_behavior_path_planner_common/package.xml +++ b/planning/autoware_behavior_path_planner_common/package.xml @@ -48,6 +48,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_rtc_interface + autoware_vehicle_info_utils geometry_msgs interpolation lanelet2_extension @@ -61,7 +62,6 @@ tier4_autoware_utils tier4_planning_msgs traffic_light_utils - vehicle_info_util visualization_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 2b150b807c6ce..a22a63ba0fe6d 100644 --- a/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -42,7 +42,7 @@ using visualization_msgs::msg::Marker; void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double half_width = vehicle_info.vehicle_width_m / 2.0; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; @@ -60,7 +60,7 @@ void addFootprintMarker( } MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); @@ -388,8 +388,9 @@ MarkerArray createDrivableLanesMarkerArray( return msg; } MarkerArray createPredictedPathMarkerArray( - const PredictedPath & predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, - std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) + const PredictedPath & predicted_path, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, std::string && ns, + const int32_t & id, const float & r, const float & g, const float & b) { if (predicted_path.path.empty()) { return MarkerArray{}; diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 233e6f47b92cc..7951be43aa799 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -51,7 +51,7 @@ bool isTargetObjectOncoming( bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_offset_pose = @@ -71,7 +71,7 @@ bool isTargetObjectFront( bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_point = @@ -90,7 +90,7 @@ bool isTargetObjectFront( } Polygon2d createExtendedPolygon( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { @@ -189,7 +189,7 @@ Polygon2d createExtendedPolygon( Polygon2d createExtendedPolygonAlongPath( const PathWithLaneId & planned_path, const Pose & base_link_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 3cb71b1f9c4e2..4f23c9f6c07c0 100644 --- a/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -39,7 +39,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; - vehicle_info_util::VehicleInfo vehicle_info; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; vehicle_info.max_longitudinal_offset_m = 4.0; vehicle_info.vehicle_width_m = 2.0; vehicle_info.rear_overhang_m = 1.0; diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp index 613911ba33460..0c8eb1850b9d5 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp @@ -36,7 +36,7 @@ class FreespacePullOut : public PullOutPlannerBase public: FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp index 8cbe3569d13fe..38eeb558e85eb 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -45,7 +45,7 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } @@ -95,7 +95,7 @@ class PullOutPlannerBase collision_check_margin_); }; std::shared_ptr planner_data_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; double collision_check_margin_; diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp index 7a98a43efec5b..ccbf1eada7378 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp @@ -28,8 +28,8 @@ #include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" #include -#include -#include +#include +#include #include @@ -255,7 +255,7 @@ class StartPlannerModule : public SceneModuleInterface mutable std::shared_ptr ego_predicted_path_params_; mutable std::shared_ptr objects_filtering_params_; mutable std::shared_ptr safety_check_params_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; diff --git a/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index f0a83741212b8..1b154138d427e 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -28,7 +28,7 @@ namespace behavior_path_planner { FreespacePullOut::FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) : PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( diff --git a/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index b30af17eaa898..2efd2bc17e6ab 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -60,7 +60,7 @@ StartPlannerModule::StartPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { lane_departure_checker_ = std::make_shared(); diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index e3699306df1f3..5a47bed4c712a 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -24,6 +24,7 @@ autoware_behavior_path_planner_common autoware_perception_msgs autoware_rtc_interface + autoware_vehicle_info_utils behavior_path_planner geometry_msgs lanelet2_extension @@ -41,7 +42,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 8671c71430b3d..1408c096c7e4e 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -138,7 +138,7 @@ double calcSignedArcLengthToFirstNearestPoint( } geometry_msgs::msg::Polygon createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double offset) { const auto & i = vehicle_info; const auto & front_m = i.max_longitudinal_offset_m; diff --git a/planning/autoware_behavior_velocity_intersection_module/package.xml b/planning/autoware_behavior_velocity_intersection_module/package.xml index 0655867951ed8..5ce1ff206a1a9 100644 --- a/planning/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/autoware_behavior_velocity_intersection_module/package.xml @@ -22,6 +22,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_rtc_interface + autoware_vehicle_info_utils fmt geometry_msgs interpolation @@ -36,7 +37,6 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml index 2f56bce7155b5..d8177ddc908f3 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -19,6 +19,7 @@ autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs grid_map_ros grid_map_utils @@ -34,7 +35,6 @@ tf2 tf2_geometry_msgs tier4_autoware_utils - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index b1757c4142212..c1d345cbc9aac 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -109,7 +109,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) pp.grid.free_space_max = getOrDeclareParameter(node, ns + ".grid.free_space_max"); pp.grid.occupied_min = getOrDeclareParameter(node, ns + ".grid.occupied_min"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.baselink_to_front = vehicle_info.max_longitudinal_offset_m; pp.wheel_tread = vehicle_info.wheel_tread_m; pp.right_overhang = vehicle_info.right_overhang_m; diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp index c69f23215a369..df5d083f5dcb1 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp @@ -18,8 +18,8 @@ #include "route_handler/route_handler.hpp" #include +#include #include -#include #include #include @@ -51,7 +51,7 @@ class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { max_stop_acceleration_threshold = node.declare_parameter( "max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml? @@ -92,7 +92,7 @@ struct PlannerData // route handler std::shared_ptr route_handler_; // parameters - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // additional parameters double max_stop_acceleration_threshold; diff --git a/planning/autoware_behavior_velocity_planner_common/package.xml b/planning/autoware_behavior_velocity_planner_common/package.xml index 7ab991a247a63..0e317ceb5db66 100644 --- a/planning/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/autoware_behavior_velocity_planner_common/package.xml @@ -24,6 +24,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_rtc_interface + autoware_vehicle_info_utils autoware_velocity_smoother diagnostic_msgs eigen @@ -46,7 +47,6 @@ tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_velocity_run_out_module/package.xml b/planning/autoware_behavior_velocity_run_out_module/package.xml index 330880b77d3da..284797cbed0a8 100644 --- a/planning/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/autoware_behavior_velocity_run_out_module/package.xml @@ -22,6 +22,7 @@ autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils behavior_velocity_crosswalk_module eigen geometry_msgs @@ -38,7 +39,6 @@ tf2_eigen tf2_ros tier4_autoware_utils - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp index 002a49abae611..4da58522165d6 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -28,7 +28,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) { // Vehicle Parameters { - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); auto & p = planner_param_.vehicle_param; p.base_to_front = vehicle_info.wheel_base_m + vehicle_info.front_overhang_m; p.base_to_rear = vehicle_info.rear_overhang_m; diff --git a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp index 28bfa9569c66d..90f8238d0d543 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -34,6 +34,7 @@ namespace autoware::behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; @@ -44,7 +45,6 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathWithLaneId; -using vehicle_info_util::VehicleInfo; using PathPointsWithLaneId = std::vector; struct CommonParam { diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index cda3abbd0eee9..415dcd983a8df 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -19,6 +19,7 @@ autoware_adapi_v1_msgs autoware_behavior_velocity_planner_common autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs lanelet2_extension motion_utils @@ -29,7 +30,6 @@ tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 9775e64145529..d413a8721c09e 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -16,13 +16,13 @@ #define SCENE_HPP_ #include +#include #include #include #include #include #include #include -#include #include #include diff --git a/planning/autoware_behavior_velocity_walkway_module/package.xml b/planning/autoware_behavior_velocity_walkway_module/package.xml index bbe87ffb8b2cd..60ef7593ab8ff 100644 --- a/planning/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/autoware_behavior_velocity_walkway_module/package.xml @@ -19,6 +19,7 @@ autoware_behavior_velocity_planner_common autoware_planning_msgs + autoware_vehicle_info_utils behavior_velocity_crosswalk_module geometry_msgs lanelet2_extension @@ -28,7 +29,6 @@ rclcpp route_handler tier4_autoware_utils - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp index 27a563f4ec1af..394900c9a704c 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ #define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#include #include -#include #include #include @@ -69,7 +69,7 @@ struct VehicleShape } explicit VehicleShape( - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : length(vehicle_info.vehicle_length_m + margin), width(vehicle_info.vehicle_width_m + margin), base2back(vehicle_info.rear_overhang_m + margin / 2.0) @@ -125,7 +125,7 @@ class AbstractPlanningAlgorithm AbstractPlanningAlgorithm( const PlannerCommonParam & planner_common_param, - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin) { } diff --git a/planning/autoware_freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml index d2b7ddc7c620a..61173f0a877e4 100644 --- a/planning/autoware_freespace_planning_algorithms/package.xml +++ b/planning/autoware_freespace_planning_algorithms/package.xml @@ -16,6 +16,7 @@ autoware_cmake python_cmake_module + autoware_vehicle_info_utils geometry_msgs nav_msgs nlohmann-json-dev @@ -25,7 +26,6 @@ tf2 tf2_geometry_msgs tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index edca8d706047b..cea58a11a530a 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -16,10 +16,10 @@ #include "autoware_path_optimizer/common_structs.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/clock.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -30,6 +30,6 @@ namespace autoware::path_optimizer MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool publish_extra_marker); } // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp index 8c207a9a3830f..98d7e131f4bdc 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp @@ -18,12 +18,12 @@ #include "autoware_path_optimizer/common_structs.hpp" #include "autoware_path_optimizer/state_equation_generator.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -105,8 +105,8 @@ class MPTOptimizer public: MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, - const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, - const std::shared_ptr debug_data_ptr, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_ptr); std::vector optimizeTrajectory(const PlannerData & planner_data); @@ -142,7 +142,8 @@ class MPTOptimizer struct MPTParam { - explicit MPTParam(rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info); + explicit MPTParam( + rclcpp::Node * node, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MPTParam() = default; void onParam(const std::vector & parameters); @@ -218,7 +219,7 @@ class MPTOptimizer // argument bool enable_debug_info_; EgoNearestParam ego_nearest_param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; mutable std::shared_ptr time_keeper_ptr_; diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp index 30c95debe11cb..c1f644d31aa9a 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp @@ -19,11 +19,11 @@ #include "autoware_path_optimizer/mpt_optimizer.hpp" #include "autoware_path_optimizer/replan_checker.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -63,7 +63,7 @@ class PathOptimizer : public rclcpp::Node DrivingDirectionChecker driving_direction_checker_{}; // argument variables - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; mutable std::shared_ptr time_keeper_ptr_{nullptr}; diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index 588b68f52a094..aa44a515a08ec 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -17,11 +17,11 @@ #include "autoware_path_optimizer/common_structs.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -65,7 +65,7 @@ bool isSamePoint(const T1 & t1, const T2 & t2) bool isOutsideDrivableAreaFromRectangleFootprint( const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, const std::vector & right_bound, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check); } // namespace geometry_utils } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index f02473ebd015e..c158815999398 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -16,6 +16,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils geometry_msgs interpolation motion_utils @@ -28,7 +29,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index 7c644f4448a0c..c97d5c5adec46 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -29,7 +29,7 @@ namespace { MarkerArray getFootprintsMarkerArray( const std::vector & mpt_traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t sampling_num) { auto marker = createDefaultMarker( "map", rclcpp::Clock().now(), "mpt_footprints", 0, Marker::LINE_STRIP, @@ -73,7 +73,7 @@ MarkerArray getFootprintsMarkerArray( MarkerArray getBoundsWidthMarkerArray( const std::vector & ref_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); MarkerArray marker_array; @@ -146,7 +146,7 @@ MarkerArray getBoundsWidthMarkerArray( MarkerArray getBoundsLineMarkerArray( const std::vector & ref_points, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { MarkerArray marker_array; @@ -183,7 +183,7 @@ MarkerArray getBoundsLineMarkerArray( MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, const std::vector & vehicle_circle_longitudinal_offsets, - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t sampling_num, const std::string & ns) { const auto current_time = rclcpp::Clock().now(); @@ -339,8 +339,9 @@ visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( } visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( - const geometry_msgs::msg::Pose & stop_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const std::string & ns, const double r, const double g, const double b) + const geometry_msgs::msg::Pose & stop_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const std::string & ns, + const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; @@ -373,7 +374,7 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool publish_extra_marker) { MarkerArray marker_array; diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 2ab622c6b4b58..0652085d85c48 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -34,7 +34,7 @@ namespace autoware::path_optimizer namespace { std::tuple, std::vector> calcVehicleCirclesByUniformCircle( - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t circle_num, const double radius_ratio) { const double lateral_offset = @@ -56,7 +56,7 @@ std::tuple, std::vector> calcVehicleCirclesByUniform } std::tuple, std::vector> calcVehicleCirclesByBicycleModel( - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t circle_num, const double rear_radius_ratio, const double front_radius_ratio) { if (circle_num < 2) { @@ -84,7 +84,7 @@ std::tuple, std::vector> calcVehicleCirclesByBicycle } std::tuple, std::vector> calcVehicleCirclesByFittingUniformCircle( - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t circle_num) { if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); @@ -169,7 +169,7 @@ double calcLateralDistToBounds( } // namespace MPTOptimizer::MPTParam::MPTParam( - rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info) + rclcpp::Node * node, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { { // option steer_limit_constraint = node->declare_parameter("mpt.option.steer_limit_constraint"); @@ -393,8 +393,8 @@ void MPTOptimizer::MPTParam::onParam(const std::vector & para MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, - const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, - const std::shared_ptr debug_data_ptr, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_ptr) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 816c0d459d95f..d9df26938a9f8 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -84,7 +84,7 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()), time_keeper_ptr_(std::make_shared()) { diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 45302c0b729a9..99eb85d27e054 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -120,7 +120,7 @@ namespace geometry_utils bool isOutsideDrivableAreaFromRectangleFootprint( const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, const std::vector & right_bound, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check) { if (left_bound.empty() || right_bound.empty()) { diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 26eb41ca1de23..a5e09ceb6270b 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -21,6 +21,7 @@ autoware_path_smoother autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils geography_utils geometry_msgs global_parameter_loader @@ -36,7 +37,6 @@ route_handler tier4_autoware_utils tier4_map_msgs - vehicle_info_util python3-flask-cors rosidl_default_runtime diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 24e95353eec93..b8a3a8baf67a3 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -77,8 +77,8 @@ lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & } LinearRing2d create_vehicle_footprint( - const geometry_msgs::msg::Pose & pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double margin = 0.0) + const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) { const auto & i = vehicle_info; @@ -105,7 +105,8 @@ LinearRing2d create_vehicle_footprint( } geometry_msgs::msg::Pose get_text_pose( - const geometry_msgs::msg::Pose & pose, const vehicle_info_util::VehicleInfo & vehicle_info) + const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const auto & i = vehicle_info; @@ -238,7 +239,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( rmw_qos_profile_services_default, callback_group_); // vehicle info - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); centerline_source_ = [&]() { const auto centerline_source_param = declare_parameter("centerline_source"); diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp index f8a65f2794809..2f25a064dca2a 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -18,10 +18,10 @@ #include "autoware_static_centerline_generator/srv/load_map.hpp" #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "centerline_source/optimization_trajectory_based_centerline.hpp" #include "rclcpp/rclcpp.hpp" #include "type_alias.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" @@ -115,7 +115,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_; // vehicle info - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; }; } // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp index d2e055bbccaa2..c7ec2e5b564e6 100644 --- a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #define AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#include #include -#include #include #include @@ -34,6 +34,7 @@ namespace autoware::surround_obstacle_checker { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; @@ -41,7 +42,6 @@ using geometry_msgs::msg::PolygonStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; -using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -56,7 +56,7 @@ class SurroundObstacleCheckerDebugNode { public: explicit SurroundObstacleCheckerDebugNode( - const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double base_link2front, const std::string & object_label, const double & surround_check_front_distance, const double & surround_check_side_distance, const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, @@ -79,7 +79,7 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; double base_link2front_; std::string object_label_; double surround_check_front_distance_; diff --git a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp index 4ccd097b40837..4eb094e6617a4 100644 --- a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp @@ -19,9 +19,9 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include #include #include -#include #include #include @@ -46,12 +46,12 @@ namespace autoware::surround_obstacle_checker { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; using Obstacle = std::pair; @@ -132,7 +132,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node // parameter NodeParam node_param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // data nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr_; diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 3bb961766e9f3..530f850a3c34a 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -17,6 +17,7 @@ autoware_adapi_v1_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils diagnostic_msgs eigen libpcl-all-dev @@ -32,7 +33,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index ba561a4b8cf35..b676673128269 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -59,7 +59,7 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( - const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double base_link2front, const std::string & object_label, const double & surround_check_front_distance, const double & surround_check_side_distance, const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 0744ebecd370c..8d6279eeefb22 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -196,7 +196,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio logger_configure_ = std::make_unique(this); } - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Publishers pub_stop_reason_ = diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 4b5a46e495545..2bfc32ec90a5f 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -22,6 +22,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils geometry_msgs interpolation motion_utils @@ -33,7 +34,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 1541b19d70a1c..be1f77fc951b1 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -20,7 +20,7 @@ #include "motion_utils/marker/marker_helper.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" -#include +#include #include #include @@ -40,7 +40,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti using std::placeholders::_1; // set common params - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; base_link2front_ = vehicle_info.max_longitudinal_offset_m; initCommonParam(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp index b1c4a705ae94d..f05d0b9b3b973 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -37,7 +37,7 @@ class FreespacePullOver : public PullOverPlannerBase public: FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index f41de932bb006..66f7274f888be 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -465,7 +465,7 @@ class GoalPlannerModule : public SceneModuleInterface mutable std::shared_ptr objects_filtering_params_; mutable std::shared_ptr safety_check_params_; - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; // planner std::vector> pull_over_planners_; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ca84e7256a01f..c916e6be10d9f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -112,7 +112,7 @@ class PullOverPlannerBase public: PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } @@ -133,7 +133,7 @@ class PullOverPlannerBase protected: std::shared_ptr planner_data_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index d6c9215e495ae..3de0ba0c99a35 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -58,8 +58,9 @@ lanelet::ConstLanelets generateExpandedPullOverLanes( lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, - const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double outer_road_offset, const double inner_road_offset); + const geometry_msgs::msg::Pose ego_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset, + const double inner_road_offset); PredictedObjects extractObjectsInExpandedPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance, double bound_offset, const PredictedObjects & objects); diff --git a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp index 360707003f7ca..0a75e5ce94f79 100644 --- a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -27,7 +27,7 @@ namespace behavior_path_planner { FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index d85205e001d0b..20fe808f51433 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -60,7 +60,7 @@ GoalPlannerModule::GoalPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_}, is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false}, @@ -95,7 +95,7 @@ GoalPlannerModule::GoalPlannerModule( // set selected goal searcher // currently there is only one goal_searcher_type - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info.createFootprint(); goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_); diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 202eefe37bd83..6b538b6e9c7f5 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -105,8 +105,9 @@ static double getOffsetToLanesBoundary( lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, - const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double outer_road_offset, const double inner_road_offset) + const geometry_msgs::msg::Pose ego_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset, + const double inner_road_offset) { const double front_overhang = vehicle_info.front_overhang_m, wheel_base = vehicle_info.wheel_base_m, wheel_tread = vehicle_info.wheel_tread_m; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index e9a9ab15f6bc0..a7f65f82f4e9e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -241,7 +241,7 @@ rclcpp::Logger getLogger(const std::string & type); * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info); + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info); /** * @brief Checks if the given polygon is within an intersection area. @@ -304,7 +304,7 @@ namespace behavior_path_planner::utils::lane_change::debug geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); geometry_msgs::msg::Polygon createExecutionArea( - const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, double additional_lat_offset); } // namespace behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 20b502886ccef..2fafd7c41d4a3 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -25,6 +25,7 @@ #include "object_recognition_utils/predicted_path_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" +#include #include #include #include @@ -33,7 +34,6 @@ #include #include #include -#include #include @@ -1159,7 +1159,7 @@ rclcpp::Logger getLogger(const std::string & type) } Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info) + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info) { const auto base_to_front = ego_info.max_longitudinal_offset_m; const auto base_to_rear = ego_info.rear_overhang_m; @@ -1240,7 +1240,7 @@ geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose }; geometry_msgs::msg::Polygon createExecutionArea( - const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, double additional_lat_offset) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 1ce6e65bee219..2c88a6d156f3d 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -45,6 +45,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils autoware_vehicle_msgs behaviortree_cpp_v3 geometry_msgs @@ -66,7 +67,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index e6ebe66f3f440..d5b49f5efa4d2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -19,8 +19,8 @@ #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" +#include #include -#include #include @@ -44,8 +44,8 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) namespace behavior_path_planner { +using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; -using vehicle_info_util::VehicleInfoUtil; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) : Node("behavior_path_planner", node_options) @@ -212,7 +212,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); // vehicle info - const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = VehicleInfoUtils(*this).getVehicleInfo(); p.vehicle_info = vehicle_info; p.vehicle_width = vehicle_info.vehicle_width_m; p.vehicle_length = vehicle_info.vehicle_length_m; diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 0a5dec8d07b4f..5cf7b1f4e927d 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -28,6 +28,7 @@ #include "autoware_sampler_common/constraints/soft_constraint.hpp" #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "behavior_path_sampling_planner_module/util.hpp" #include "lanelet2_extension/utility/query.hpp" @@ -39,7 +40,6 @@ #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" @@ -248,7 +248,7 @@ class SamplingPlannerModule : public SceneModuleInterface // member // std::shared_ptr params_; std::shared_ptr internal_params_; - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; std::optional prev_sampling_path_ = std::nullopt; // move to utils diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index 0583056db4293..c48f3240ab84d 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -20,6 +20,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs interpolation @@ -37,7 +38,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 22187652bd663..d2cc57d46de30 100644 --- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -35,7 +35,7 @@ SamplingPlannerModule::SamplingPlannerModule( std::unordered_map> & objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { internal_params_ = std::make_shared(); updateModuleParams(parameters); diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index eebf9c4d4a14f..87ccdebd02fcf 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -24,6 +24,7 @@ autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils eigen geometry_msgs grid_map_core @@ -42,7 +43,6 @@ tier4_api_msgs tier4_autoware_utils tier4_debug_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 91893c5224550..0db87508975a0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1207,7 +1207,7 @@ geometry_msgs::msg::Polygon CrosswalkModule::createObjectPolygon( } geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const auto & i = vehicle_info; const auto & front_m = i.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 13ecddafb6490..75f75846d4f4b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -419,7 +419,7 @@ class CrosswalkModule : public SceneModuleInterface const double width_m, const double length_m); static geometry_msgs::msg::Polygon createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); void recordTime(const int step_num) { diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml index 6b9db657d6d8e..f25ded5e1823c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -15,6 +15,7 @@ autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs libboost-dev motion_utils @@ -24,7 +25,6 @@ tf2 tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index ddb8c842fb92d..f594432c9e452 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -43,7 +43,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node pp.ignore_unavoidable_collisions = getOrDeclareParameter(node, ns + ".ignore_unavoidable_collisions"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.ego_lateral_offset = std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index baa51d6681490..97b3b01ee31e1 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils eigen geometry_msgs interpolation @@ -34,7 +35,6 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index 6aeac1e9d8dd3..31682fe0419d2 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -39,7 +39,7 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & pp = planner_param_; - const auto & vi = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto & vi = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); pp.stuck_vehicle_vel_thr = getOrDeclareParameter(node, ns + ".stuck_vehicle_vel_thr"); pp.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp index fbaef46c43511..22205ce427632 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -35,8 +35,8 @@ #include #include +#include #include -#include #include #include diff --git a/planning/freespace_planner/launch/freespace_planner.launch.xml b/planning/freespace_planner/launch/freespace_planner.launch.xml index 7c2408b0311f5..3c8cce7a93941 100644 --- a/planning/freespace_planner/launch/freespace_planner.launch.xml +++ b/planning/freespace_planner/launch/freespace_planner.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index a29202060a640..06004ce9f308f 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -18,6 +18,7 @@ autoware_freespace_planning_algorithms autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils geometry_msgs motion_utils nav_msgs @@ -29,7 +30,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 322bf2697a1f6..f17b9227c89bc 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -241,7 +241,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti // set vehicle_info { - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_shape_.length = vehicle_info.vehicle_length_m; vehicle_shape_.width = vehicle_info.vehicle_width_m; vehicle_shape_.base2back = vehicle_info.rear_overhang_m; diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f878953d50420..6782736c3e173 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_map_msgs autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs lanelet2_extension motion_utils @@ -31,7 +32,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 5ba152af6611d..a7f3cfaa7ee34 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -16,6 +16,7 @@ #include "utility_functions.hpp" +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include #include @@ -108,7 +108,7 @@ double project_goal_to_map( geometry_msgs::msg::Pose get_closest_centerline_pose( const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, - vehicle_info_util::VehicleInfo vehicle_info) + autoware::vehicle_info_utils::VehicleInfo vehicle_info) { lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLaneletWithConstrains( @@ -152,7 +152,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) pub_goal_footprint_marker_ = node_->create_publisher("~/debug/goal_footprint", durable_qos); - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); param_.enable_correct_goal_pose = node_->declare_parameter("enable_correct_goal_pose"); param_.consider_no_drivable_lanes = node_->declare_parameter("consider_no_drivable_lanes"); diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 60c232162991d..49000a750a9a1 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -15,10 +15,10 @@ #ifndef LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ #define LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ +#include #include #include #include -#include #include #include @@ -52,7 +52,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: using RouteSections = std::vector; diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 428cd979a2526..ed8dbfed63bfe 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,10 +15,10 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ +#include #include #include #include -#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 051415e9856ed..a6176555ae7af 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -17,6 +17,7 @@ autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs lanelet2_extension libboost-dev @@ -28,7 +29,6 @@ tier4_autoware_utils tier4_planning_msgs traffic_light_utils - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 0fe7b783adf31..11640f9ad2c8e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -100,7 +100,7 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; pp.left_offset = vehicle_info.max_lateral_offset_m; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index 73d71e0d0f955..a25ab5d55a910 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#include #include #include -#include #include #include @@ -54,7 +54,7 @@ struct TrafficSignalStamped struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { } @@ -81,7 +81,7 @@ struct PlannerData // velocity smoother std::shared_ptr velocity_smoother_{}; // parameters - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; /** *@fn diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 3aea741e3b2f4..8f9724d9c8bed 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -53,7 +53,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // main functions std::vector createOneStepPolygons( const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; std::vector convertToObstacles( const Odometry & odometry, const PredictedObjects & objects, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 44626206ec038..a832e74c76db0 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -15,11 +15,11 @@ #ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ #define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -35,8 +35,8 @@ class OptimizationBasedPlanner : public PlannerInterface public: OptimizationBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr); std::vector generateCruiseTrajectory( const PlannerData & planner_data, const std::vector & stop_traj_points, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index 26fd3a8ce2ce2..9d688cdac419b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -49,8 +49,8 @@ class PIDBasedPlanner : public PlannerInterface PIDBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr); std::vector generateCruiseTrajectory( const PlannerData & planner_data, const std::vector & stop_traj_points, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 7a060657e16af..f3dadbf5e5e51 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -38,8 +38,8 @@ class PlannerInterface public: PlannerInterface( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) : longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), @@ -135,7 +135,7 @@ class PlannerInterface rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; // Vehicle Parameters - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; EgoNearestParam ego_nearest_param_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 71d0f84637abc..455d40a7a7e87 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -15,10 +15,10 @@ #ifndef OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ #define OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -36,12 +36,12 @@ using tier4_autoware_utils::Polygon2d; Polygon2d createOneStepPolygon( const std::vector & last_poses, const std::vector & current_poses, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lat_margin); std::optional> getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); std::vector getCollisionPoints( const std::vector & traj_points, const std::vector & traj_polygons, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index 979cef8610cbd..196526c22aac9 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -15,7 +15,7 @@ #ifndef OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ #define OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ -#include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" #include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" @@ -35,6 +35,7 @@ #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +using autoware::vehicle_info_utils::VehicleInfo; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; @@ -56,7 +57,6 @@ using tier4_planning_msgs::msg::StopReasonArray; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; diff --git a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml index a6c95d65acc6a..8db1ae8b46223 100644 --- a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml +++ b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index e394add65b05d..2a6e240093b14 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -21,6 +21,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils geometry_msgs interpolation lanelet2_extension @@ -37,7 +38,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 41ae29b7d470f..866090a3c4696 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -353,7 +353,7 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()) { using std::placeholders::_1; @@ -558,7 +558,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const { const auto & p = behavior_determination_param_; diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 1b5120eba0a11..a040ac598681f 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -38,8 +38,8 @@ constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; OptimizationBasedPlanner::OptimizationBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr) { smoothed_traj_sub_ = node.create_subscription( diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index a5afbe74cc9c4..3084764432c0e 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -58,8 +58,8 @@ T getSign(const T val) PIDBasedPlanner::PIDBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr) { min_accel_during_cruise_ = diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 1bd2de0bd985c..f354c53365dc1 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -95,7 +95,7 @@ namespace polygon_utils std::optional> getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const auto collision_info = getCollisionIndex(traj_points, traj_polygons, obstacle.pose, obstacle.stamp, obstacle.shape); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 723d7e8d12e56..d7fc83dbdf7ed 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -21,13 +21,13 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include #include #include #include #include #include #include -#include #include #include @@ -77,6 +77,7 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -90,7 +91,6 @@ using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index b43e51873c09e..7b30ac61af46f 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -18,8 +18,8 @@ #include "obstacle_stop_planner/planner_data.hpp" +#include #include -#include #include #include @@ -45,10 +45,10 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using std_msgs::msg::Header; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml index bec3aef6b4558..db38370d36dfb 100644 --- a/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml +++ b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index fcf09b96c1345..39195f2840b81 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -24,6 +24,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils diagnostic_msgs motion_utils nav_msgs @@ -41,7 +42,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 31c044c93af22..3a269d738e02d 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -56,7 +56,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod : Node("obstacle_stop_planner", node_options) { // Vehicle Parameters - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); const auto & i = vehicle_info_; diff --git a/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml b/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml index 4781ceda528bf..cb6cab3b125e2 100644 --- a/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml +++ b/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index 49e424bfbc9ef..ad83f397902fc 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -14,6 +14,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils eigen grid_map_msgs grid_map_ros @@ -31,7 +32,6 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 18cf534da8c7e..4b28d9a62759b 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -22,12 +22,12 @@ #include "obstacle_velocity_limiter/trajectory_preprocessing.hpp" #include "obstacle_velocity_limiter/types.hpp" +#include #include #include #include #include #include -#include #include @@ -71,7 +71,7 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); vehicle_front_offset_ = static_cast(vehicle_info.max_longitudinal_offset_m); diff --git a/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp index 113c70fecb62c..fa0b116eafe86 100644 --- a/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -17,9 +17,9 @@ #include "autoware_planning_validator/debug_marker.hpp" #include "autoware_planning_validator/msg/planning_validator_status.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -124,7 +124,7 @@ class PlanningValidator : public rclcpp::Node PlanningValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool isAllValid(const PlanningValidatorStatus & status) const; diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 7fe57e68928d9..541396b52b02b 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -17,6 +17,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils diagnostic_updater geometry_msgs motion_utils @@ -24,7 +25,6 @@ rclcpp rclcpp_components tier4_autoware_utils - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index aac50e24c5d5c..420ee564cda72 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -92,7 +92,7 @@ void PlanningValidator::setupParameters() } try { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); } catch (...) { RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); vehicle_info_.front_overhang_m = 0.5; diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp index e44c38aed2b53..143b9ee30b2c2 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp @@ -19,8 +19,8 @@ #include "autoware_path_sampler/parameters.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -39,7 +39,7 @@ class PathSampler : public rclcpp::Node protected: // for the static_centerline_generator package // argument variables - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable DebugData debug_data_{}; mutable std::shared_ptr time_keeper_ptr_{nullptr}; diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index abd2fa367bc7b..b6c63eeffed18 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -17,12 +17,12 @@ #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 011da8d469607..a48e6c0b836bc 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -17,6 +17,7 @@ autoware_frenet_planner autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs interpolation motion_utils @@ -28,7 +29,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index a82c1b6a52a26..9a828d23af5cb 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -60,7 +60,7 @@ bool hasZeroVelocity(const TrajectoryPoint & traj_point) PathSampler::PathSampler(const rclcpp::NodeOptions & node_options) : Node("path_sampler", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), time_keeper_ptr_(std::make_shared()) { // interface publisher diff --git a/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py b/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py index 175209d49fa8b..98bc5a6dd2699 100644 --- a/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py +++ b/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): pkg = "pointcloud_preprocessor" param_file = os.path.join( - get_package_share_directory("vehicle_info_util"), "config/polygon_remover.yaml" + get_package_share_directory("autoware_vehicle_info_utils"), "config/polygon_remover.yaml" ) with open(param_file, "r") as f: diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index a8eb88f55df19..fd01f42928947 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -91,7 +91,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "vehicle_info_param_file", [ - FindPackageShare("vehicle_info_util"), + FindPackageShare("autoware_vehicle_info_utils"), "/config/vehicle_info.param.yaml", ], "path to the parameter file of vehicle information", diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 5f04fba4fdb8e..b820c5c6c3df0 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -18,6 +18,7 @@ autoware_control_msgs autoware_map_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs lanelet2_core @@ -35,7 +36,6 @@ tier4_autoware_utils tier4_external_api_msgs tier4_vehicle_msgs - vehicle_info_util autoware_cmake launch_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 3b65218aaf03d..c66a0c6f1f922 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -14,13 +14,13 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -232,7 +232,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() const double debug_acc_scaling_factor = declare_parameter("debug_acc_scaling_factor", 1.0); const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; std::vector model_module_paths = declare_parameter>( diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 77276c7acb0e3..315fdea27a3f3 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -17,6 +17,7 @@ autoware_adapi_version_msgs autoware_planning_msgs autoware_system_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -32,7 +33,6 @@ tier4_api_msgs tier4_control_msgs tier4_system_msgs - vehicle_info_util python3-flask diff --git a/system/default_ad_api/src/vehicle_info.cpp b/system/default_ad_api/src/vehicle_info.cpp index cff7ac5cd5eae..8bf7da65f6e33 100644 --- a/system/default_ad_api/src/vehicle_info.cpp +++ b/system/default_ad_api/src/vehicle_info.cpp @@ -14,7 +14,7 @@ #include "vehicle_info.hpp" -#include +#include namespace { @@ -41,7 +41,7 @@ VehicleInfoNode::VehicleInfoNode(const rclcpp::NodeOptions & options) res->dimensions = dimensions_; }; - const auto vehicle = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); dimensions_.wheel_radius = vehicle.wheel_radius_m; dimensions_.wheel_width = vehicle.wheel_width_m; dimensions_.wheel_base = vehicle.wheel_base_m; diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml index 4246f8dee8a4e..08db4f5903abe 100644 --- a/vehicle/autoware_steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs @@ -17,7 +18,6 @@ std_msgs tier4_autoware_utils tier4_debug_msgs - vehicle_info_util global_parameter_loader pose2twist diff --git a/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp index fdb5fdb404c0c..375e989f8f201 100644 --- a/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp +++ b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp @@ -14,7 +14,7 @@ #include "autoware_steer_offset_estimator/steer_offset_estimator_node.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include #include @@ -26,7 +26,7 @@ SteerOffsetEstimatorNode::SteerOffsetEstimatorNode(const rclcpp::NodeOptions & n { using std::placeholders::_1; // get parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); wheel_base_ = vehicle_info.wheel_base_m; covariance_ = this->declare_parameter("initial_covariance"); forgetting_factor_ = this->declare_parameter("forgetting_factor"); diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/vehicle/autoware_vehicle_info_utils/CMakeLists.txt similarity index 71% rename from vehicle/vehicle_info_util/CMakeLists.txt rename to vehicle/autoware_vehicle_info_utils/CMakeLists.txt index ca72b0ed2db65..6d090ab3ebff7 100644 --- a/vehicle/vehicle_info_util/CMakeLists.txt +++ b/vehicle/autoware_vehicle_info_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(vehicle_info_util) +project(autoware_vehicle_info_utils) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(vehicle_info_util SHARED +ament_auto_add_library(vehicle_info_utils SHARED src/vehicle_info.cpp - src/vehicle_info_util.cpp + src/vehicle_info_utils.cpp ) ament_auto_package( diff --git a/vehicle/vehicle_info_util/Readme.md b/vehicle/autoware_vehicle_info_utils/Readme.md similarity index 89% rename from vehicle/vehicle_info_util/Readme.md rename to vehicle/autoware_vehicle_info_utils/Readme.md index 600fd62270d81..537dc04aa8c69 100644 --- a/vehicle/vehicle_info_util/Readme.md +++ b/vehicle/autoware_vehicle_info_utils/Readme.md @@ -42,15 +42,15 @@ We have implemented a versioning system for the `vehicle_info.param.yaml` file t #### Minimum turning radius ```sh -$ ros2 run vehicle_info_util min_turning_radius_calculator.py -yaml path is /home/autoware/pilot-auto/install/vehicle_info_util/share/vehicle_info_util/config/vehicle_info.param.yaml +$ ros2 run autoware_vehicle_info_utils min_turning_radius_calculator.py +yaml path is /home/autoware/pilot-auto/install/autoware_vehicle_info_utils/share/autoware_vehicle_info_utils/config/vehicle_info.param.yaml Minimum turning radius is 3.253042620027102 [m] for rear, 4.253220695862465 [m] for front. ``` You can designate yaml file with `-y` option as follows. ```sh -ros2 run vehicle_info_util min_turning_radius_calculator.py -y +ros2 run autoware_vehicle_info_utils min_turning_radius_calculator.py -y ``` ## Assumptions / Known limits diff --git a/vehicle/vehicle_info_util/config/polygon_remover.yaml b/vehicle/autoware_vehicle_info_utils/config/polygon_remover.yaml similarity index 100% rename from vehicle/vehicle_info_util/config/polygon_remover.yaml rename to vehicle/autoware_vehicle_info_utils/config/polygon_remover.yaml diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/vehicle/autoware_vehicle_info_utils/config/vehicle_info.param.yaml similarity index 100% rename from vehicle/vehicle_info_util/config/vehicle_info.param.yaml rename to vehicle/autoware_vehicle_info_utils/config/vehicle_info.param.yaml diff --git a/vehicle/vehicle_info_util/config/vehicle_mirror.param.yaml b/vehicle/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml similarity index 100% rename from vehicle/vehicle_info_util/config/vehicle_mirror.param.yaml rename to vehicle/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp similarity index 89% rename from vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp rename to vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp index cdbbe0427b145..c94ea01224da2 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ -#define VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ +#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ +#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { /// Data class for vehicle info struct VehicleInfo @@ -59,6 +59,6 @@ VehicleInfo createVehicleInfo( const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m, const double max_steer_angle_rad); -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils -#endif // VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ +#endif // AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp similarity index 71% rename from vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp rename to vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp index 5e1623ada5ed3..d3ca339f78451 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_ -#define VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_ +#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ +#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ -#include "vehicle_info_util/vehicle_info.hpp" +#include "autoware_vehicle_info_utils/vehicle_info.hpp" #include -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { /// This is a convenience class for saving you from declaring all parameters /// manually and calculating derived parameters. /// This class supposes that necessary parameters are set when the node is launched. -class VehicleInfoUtil +class VehicleInfoUtils { public: /// Constructor - explicit VehicleInfoUtil(rclcpp::Node & node); + explicit VehicleInfoUtils(rclcpp::Node & node); /// Get vehicle info VehicleInfo getVehicleInfo(); @@ -38,6 +38,6 @@ class VehicleInfoUtil VehicleInfo vehicle_info_; }; -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils -#endif // VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_ +#endif // AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ diff --git a/vehicle/vehicle_info_util/launch/sample.launch.py b/vehicle/autoware_vehicle_info_utils/launch/sample.launch.py similarity index 91% rename from vehicle/vehicle_info_util/launch/sample.launch.py rename to vehicle/autoware_vehicle_info_utils/launch/sample.launch.py index c0be2bc4c4c48..5f83bc695230e 100644 --- a/vehicle/vehicle_info_util/launch/sample.launch.py +++ b/vehicle/autoware_vehicle_info_utils/launch/sample.launch.py @@ -30,14 +30,14 @@ def launch_setup(context, *args, **kwargs): set_use_sim_time = SetParameter(name="use_sim_time", value=LaunchConfiguration("use_sim_time")) vehicle_info_param_path = os.path.join( - get_package_share_directory("vehicle_info_util"), + get_package_share_directory("autoware_vehicle_info_utils"), "config", "vehicle_info.param.yaml", ) # vehicle_info load_vehicle_info = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("vehicle_info_util"), "/launch/vehicle_info.launch.py"] + [FindPackageShare("autoware_vehicle_info_utils"), "/launch/vehicle_info.launch.py"] ), launch_arguments={"vehicle_info_param_file": [vehicle_info_param_path]}.items(), ) diff --git a/vehicle/vehicle_info_util/launch/vehicle_info.launch.py b/vehicle/autoware_vehicle_info_utils/launch/vehicle_info.launch.py similarity index 100% rename from vehicle/vehicle_info_util/launch/vehicle_info.launch.py rename to vehicle/autoware_vehicle_info_utils/launch/vehicle_info.launch.py diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/autoware_vehicle_info_utils/package.xml similarity index 88% rename from vehicle/vehicle_info_util/package.xml rename to vehicle/autoware_vehicle_info_utils/package.xml index 139f8439d60c9..bd8ec0cb601a0 100644 --- a/vehicle/vehicle_info_util/package.xml +++ b/vehicle/autoware_vehicle_info_utils/package.xml @@ -1,9 +1,9 @@ - vehicle_info_util + autoware_vehicle_info_utils 0.1.0 - The vehicle_info_util package + The autoware_vehicle_info_utils package Taiki Tanaka diff --git a/vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py b/vehicle/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py similarity index 93% rename from vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py rename to vehicle/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py index 9e25b928c8b74..0b581786b25ad 100755 --- a/vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py +++ b/vehicle/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py @@ -41,7 +41,8 @@ def main(yaml_path): if __name__ == "__main__": default_yaml_path = ( - get_package_share_directory("vehicle_info_util") + "/config/vehicle_info.param.yaml" + get_package_share_directory("autoware_vehicle_info_utils") + + "/config/vehicle_info.param.yaml" ) parser = argparse.ArgumentParser() diff --git a/vehicle/vehicle_info_util/src/vehicle_info.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp similarity index 95% rename from vehicle/vehicle_info_util/src/vehicle_info.cpp rename to vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp index 1a47252f82d04..c7f587d20b2f4 100644 --- a/vehicle/vehicle_info_util/src/vehicle_info.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_info_util/vehicle_info.hpp" +#include "autoware_vehicle_info_utils/vehicle_info.hpp" -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const { @@ -85,4 +85,4 @@ VehicleInfo createVehicleInfo( }; } -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils diff --git a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp similarity index 90% rename from vehicle/vehicle_info_util/src/vehicle_info_util.cpp rename to vehicle/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp index 262a4b21c0cf7..e313391f6207a 100644 --- a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include @@ -36,9 +36,9 @@ T getParameter(rclcpp::Node & node, const std::string & name) } } // namespace -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { -VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node) +VehicleInfoUtils::VehicleInfoUtils(rclcpp::Node & node) { vehicle_info_.wheel_radius_m = getParameter(node, "wheel_radius"); vehicle_info_.wheel_width_m = getParameter(node, "wheel_width"); @@ -52,7 +52,7 @@ VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node) vehicle_info_.max_steer_angle_rad = getParameter(node, "max_steer_angle"); } -VehicleInfo VehicleInfoUtil::getVehicleInfo() +VehicleInfo VehicleInfoUtils::getVehicleInfo() { return createVehicleInfo( vehicle_info_.wheel_radius_m, vehicle_info_.wheel_width_m, vehicle_info_.wheel_base_m, @@ -60,4 +60,4 @@ VehicleInfo VehicleInfoUtil::getVehicleInfo() vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m, vehicle_info_.max_steer_angle_rad); } -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils