diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml index fb533137..6ac5b532 100644 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake autoware_adapi_v1_msgs + autoware_universe_utils geometry_msgs libqt5-core libqt5-gui @@ -24,7 +25,6 @@ rviz_default_plugins tf2 tf2_geometry_msgs - tier4_autoware_utils visualization_msgs yaml-cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp index 6b8d7765..5a9ebb8b 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -16,7 +16,7 @@ #include "automatic_goal_panel.hpp" -#include +#include namespace rviz_plugins { @@ -439,9 +439,9 @@ void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const void AutowareAutomaticGoalPanel::publishMarkers() { - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; + using autoware_universe_utils::createDefaultMarker; + using autoware_universe_utils::createMarkerColor; + using autoware_universe_utils::createMarkerScale; MarkerArray text_array; MarkerArray arrow_array; diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml index bec78de0..b6396858 100644 --- a/common/tier4_control_rviz_plugin/package.xml +++ b/common/tier4_control_rviz_plugin/package.xml @@ -11,6 +11,7 @@ autoware_cmake autoware_control_msgs + autoware_universe_utils autoware_vehicle_msgs libqt5-core libqt5-gui @@ -19,7 +20,6 @@ rviz_common rviz_default_plugins rviz_ogre_vendor - tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index 3a028c90..7c65a9ca 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -11,12 +11,12 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils geometry_msgs motion_utils rclcpp rclcpp_components tf2_ros - tier4_autoware_utils tier4_debug_msgs launch_ros diff --git a/common/tier4_logging_level_configure_rviz_plugin/README.md b/common/tier4_logging_level_configure_rviz_plugin/README.md index ed400e52..15d942d0 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/README.md +++ b/common/tier4_logging_level_configure_rviz_plugin/README.md @@ -16,7 +16,7 @@ This plugin dispatches services to the "logger name" associated with "nodes" spe !!! note - As of November 2023, in ROS 2 Humble, users are required to initiate a service server in the node to use this feature. (This might be integrated into ROS standards in the future.) For easy service server generation, you can use the [LoggerLevelConfigure](https://github.com/autowarefoundation/autoware.universe/blob/main/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp) utility. + As of November 2023, in ROS 2 Humble, users are required to initiate a service server in the node to use this feature. (This might be integrated into ROS standards in the future.) For easy service server generation, you can use the [LoggerLevelConfigure](https://github.com/autowarefoundation/autoware.universe/blob/main/common/autoware_universe_utils/include/autoware_universe_utils/ros/logger_level_configure.hpp) utility. ## How to use the plugin diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index a9c7c0f7..fef2a185 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -32,7 +32,7 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: behavior_path_planner.path_shifter - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner @@ -72,7 +72,7 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner logger_name: motion_utils @@ -84,7 +84,7 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner logger_name: motion_utils @@ -92,7 +92,7 @@ Planning: - node_name: /planning/scenario_planning/motion_velocity_smoother logger_name: planning.scenario_planning.motion_velocity_smoother - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/motion_velocity_smoother logger_name: motion_utils @@ -108,7 +108,7 @@ Control: - node_name: /control/trajectory_follower/controller_node_exe logger_name: control.trajectory_follower.controller_node_exe.lateral_controller - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /control/trajectory_follower/controller_node_exe logger_name: motion_utils @@ -116,7 +116,7 @@ Control: - node_name: /control/trajectory_follower/controller_node_exe logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /control/trajectory_follower/controller_node_exe logger_name: motion_utils @@ -124,25 +124,25 @@ Control: - node_name: /control/vehicle_cmd_gate logger_name: control.vehicle_cmd_gate - node_name: /control/vehicle_cmd_gate - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils # ============================================================ # common # ============================================================ Common: - tier4_autoware_utils: + autoware_universe_utils: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/motion_planning/path_smoother - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils - node_name: /control/vehicle_cmd_gate - logger_name: tier4_autoware_utils + logger_name: autoware_universe_utils diff --git a/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp index 3c7c10ef..1867213b 100644 --- a/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp +++ b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp @@ -15,9 +15,9 @@ #ifndef STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_ #define STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_ +#include "autoware/universe_utils/ros/self_pose_listener.hpp" #include "rclcpp/rclcpp.hpp" #include "signal_processing/lowpass_filter_1d.hpp" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -30,10 +30,10 @@ namespace stop_accel_evaluator { +using autoware_universe_utils::SelfPoseListener; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; -using tier4_autoware_utils::SelfPoseListener; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; diff --git a/control/stop_accel_evaluator/package.xml b/control/stop_accel_evaluator/package.xml index 7eeae0e4..903e1e31 100644 --- a/control/stop_accel_evaluator/package.xml +++ b/control/stop_accel_evaluator/package.xml @@ -10,6 +10,7 @@ autoware_cmake + autoware_universe_utils geometry_msgs nav_msgs rclcpp @@ -17,7 +18,6 @@ sensor_msgs signal_processing std_msgs - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp b/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp index 33922aed..aab9ecff 100644 --- a/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp +++ b/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp @@ -14,7 +14,7 @@ #include "stop_accel_evaluator/stop_accel_evaluator_node.hpp" -#include "tier4_autoware_utils/math/constants.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include #include @@ -123,7 +123,7 @@ void StopAccelEvaluatorNode::calculateStopAccel() const double pitch = lpf_pitch_->filter(getPitchByQuaternion(current_pose_ptr->pose.orientation)); - stop_accel_ = accel_sum / accel_num + tier4_autoware_utils::gravity * + stop_accel_ = accel_sum / accel_num + autoware_universe_utils::gravity * std::sin(pitch); // consider removing gravity stop_accel_with_gravity_ = accel_sum / accel_num; // not consider removing gravity diff --git a/driving_environment_analyzer/package.xml b/driving_environment_analyzer/package.xml index 087d3c18..542ca888 100644 --- a/driving_environment_analyzer/package.xml +++ b/driving_environment_analyzer/package.xml @@ -20,6 +20,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_msgs geometry_msgs interpolation @@ -42,7 +43,6 @@ tf2_geometry_msgs tf2_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs tier4_rtc_msgs visualization_msgs diff --git a/driving_environment_analyzer/src/utils.cpp b/driving_environment_analyzer/src/utils.cpp index 28784753..33645722 100644 --- a/driving_environment_analyzer/src/utils.cpp +++ b/driving_environment_analyzer/src/utils.cpp @@ -54,9 +54,9 @@ std::vector calcElevationAngle(const T & points) } for (size_t i = 0; i < points.size() - 1; ++i) { - const auto p1 = tier4_autoware_utils::getPoint(points.at(i)); - const auto p2 = tier4_autoware_utils::getPoint(points.at(i + 1)); - elevation_vec.at(i) = tier4_autoware_utils::calcElevationAngle(p1, p2); + const auto p1 = autoware_universe_utils::getPoint(points.at(i)); + const auto p2 = autoware_universe_utils::getPoint(points.at(i + 1)); + elevation_vec.at(i) = autoware_universe_utils::calcElevationAngle(p1, p2); } elevation_vec.at(elevation_vec.size() - 1) = elevation_vec.at(elevation_vec.size() - 2); @@ -81,9 +81,9 @@ double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose) const size_t idx = motion_utils::findNearestSegmentIndex(points, pose.position); - const auto p1 = tier4_autoware_utils::getPoint(points.at(idx)); - const auto p2 = tier4_autoware_utils::getPoint(points.at(idx + 1)); - return tier4_autoware_utils::calcElevationAngle(p1, p2); + const auto p1 = autoware_universe_utils::getPoint(points.at(idx)); + const auto p2 = autoware_universe_utils::getPoint(points.at(idx + 1)); + return autoware_universe_utils::calcElevationAngle(p1, p2); } double getLaneWidth(const lanelet::ConstLanelet & lane) diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/tier4_autoware_utils.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/autoware_universe_utils.hpp similarity index 63% rename from localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/tier4_autoware_utils.hpp rename to localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/autoware_universe_utils.hpp index 7e7fea9e..cc339493 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/tier4_autoware_utils.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/autoware_universe_utils.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DEVIATION_ESTIMATOR__TIER4_AUTOWARE_UTILS_HPP_ -#define DEVIATION_ESTIMATOR__TIER4_AUTOWARE_UTILS_HPP_ +#ifndef DEVIATION_ESTIMATOR__AUTOWARE_UNIVERSE_UTILS_HPP_ +#define DEVIATION_ESTIMATOR__AUTOWARE_UNIVERSE_UTILS_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include -// ToDo (kminoda): Replace these functions with the one from tier4_autoware_utils. +// ToDo (kminoda): Replace these functions with the one from autoware_universe_utils. // Currently these functions are declared here since this tool has to be compatible with older // version of Autoware. @@ -38,11 +38,11 @@ template bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) { // check the first point direction - const double src_yaw = tf2::getYaw(tier4_autoware_utils::getPose(src_pose).orientation); - const double pose_direction_yaw = tier4_autoware_utils::calcAzimuthAngle( - tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); - return std::fabs(tier4_autoware_utils::normalizeRadian(src_yaw - pose_direction_yaw)) < - tier4_autoware_utils::pi / 2.0; + const double src_yaw = tf2::getYaw(autoware_universe_utils::getPose(src_pose).orientation); + const double pose_direction_yaw = autoware_universe_utils::calcAzimuthAngle( + autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose)); + return std::fabs(autoware_universe_utils::normalizeRadian(src_yaw - pose_direction_yaw)) < + autoware_universe_utils::pi / 2.0; } /** @@ -56,8 +56,8 @@ template geometry_msgs::msg::Point calcInterpolatedPoint( const Point1 & src, const Point2 & dst, const double ratio) { - const auto src_point = tier4_autoware_utils::getPoint(src); - const auto dst_point = tier4_autoware_utils::getPoint(dst); + const auto src_point = autoware_universe_utils::getPoint(src); + const auto dst_point = autoware_universe_utils::getPoint(dst); tf2::Vector3 src_vec; src_vec.setX(src_point.x); @@ -101,33 +101,35 @@ geometry_msgs::msg::Pose calcInterpolatedPose( geometry_msgs::msg::Pose output_pose; output_pose.position = calcInterpolatedPoint( - tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose), + autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose), clamped_ratio); if (set_orientation_from_position_direction) { - const double input_poses_dist = tier4_autoware_utils::calcDistance2d( - tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); + const double input_poses_dist = autoware_universe_utils::calcDistance2d( + autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose)); const bool is_driving_forward = isDrivingForward(src_pose, dst_pose); // Get orientation from interpolated point and src_pose if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) { - output_pose.orientation = tier4_autoware_utils::getPose(dst_pose).orientation; + output_pose.orientation = autoware_universe_utils::getPose(dst_pose).orientation; } else if (!is_driving_forward && clamped_ratio < 1e-6) { - output_pose.orientation = tier4_autoware_utils::getPose(src_pose).orientation; + output_pose.orientation = autoware_universe_utils::getPose(src_pose).orientation; } else { const auto & base_pose = is_driving_forward ? dst_pose : src_pose; - const double pitch = tier4_autoware_utils::calcElevationAngle( - tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); - const double yaw = tier4_autoware_utils::calcAzimuthAngle( - tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); - output_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + const double pitch = autoware_universe_utils::calcElevationAngle( + autoware_universe_utils::getPoint(output_pose), + autoware_universe_utils::getPoint(base_pose)); + const double yaw = autoware_universe_utils::calcAzimuthAngle( + autoware_universe_utils::getPoint(output_pose), + autoware_universe_utils::getPoint(base_pose)); + output_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } } else { // Get orientation by spherical linear interpolation tf2::Transform src_tf; tf2::Transform dst_tf; - tf2::fromMsg(tier4_autoware_utils::getPose(src_pose), src_tf); - tf2::fromMsg(tier4_autoware_utils::getPose(dst_pose), dst_tf); + tf2::fromMsg(autoware_universe_utils::getPose(src_pose), src_tf); + tf2::fromMsg(autoware_universe_utils::getPose(dst_pose), dst_tf); const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio); output_pose.orientation = tf2::toMsg(quaternion); } @@ -135,4 +137,4 @@ geometry_msgs::msg::Pose calcInterpolatedPose( return output_pose; } -#endif // DEVIATION_ESTIMATOR__TIER4_AUTOWARE_UTILS_HPP_ +#endif // DEVIATION_ESTIMATOR__AUTOWARE_UNIVERSE_UTILS_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp index bf773638..a3a43155 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp @@ -15,6 +15,7 @@ #ifndef DEVIATION_ESTIMATOR__DEVIATION_ESTIMATOR_HPP_ #define DEVIATION_ESTIMATOR__DEVIATION_ESTIMATOR_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "deviation_estimator/gyro_bias_module.hpp" #include "deviation_estimator/logger.hpp" #include "deviation_estimator/utils.hpp" @@ -22,7 +23,6 @@ #include "deviation_estimator/velocity_coef_module.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" @@ -103,7 +103,7 @@ class DeviationEstimator : public rclcpp::Node std::unique_ptr vel_coef_module_; std::unique_ptr validation_module_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp index 122c38b4..bf4a2f0a 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp @@ -15,7 +15,7 @@ #ifndef DEVIATION_ESTIMATOR__UTILS_HPP_ #define DEVIATION_ESTIMATOR__UTILS_HPP_ -#include "deviation_estimator/tier4_autoware_utils.hpp" +#include "deviation_estimator/autoware_universe_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/localization/deviation_estimation_tools/deviation_estimator/package.xml b/localization/deviation_estimation_tools/deviation_estimator/package.xml index 21cc5504..5962fe8e 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/package.xml +++ b/localization/deviation_estimation_tools/deviation_estimator/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils autoware_vehicle_msgs geometry_msgs nav_msgs @@ -21,7 +22,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_gtest diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp index cc3f255c..73360f4a 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -14,10 +14,10 @@ #include "deviation_estimator/deviation_estimator.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "deviation_estimator/logger.hpp" #include "deviation_estimator/utils.hpp" #include "rclcpp/logging.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -178,7 +178,7 @@ DeviationEstimator::DeviationEstimator( declare_parameter("thres_coef_vx"), declare_parameter("thres_stddev_vx"), declare_parameter("thres_bias_gyro"), declare_parameter("thres_stddev_gyro"), 5); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); RCLCPP_INFO(this->get_logger(), "[Deviation Estimator] launch success"); } diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/gyro_bias_module.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/gyro_bias_module.cpp index 82ef0058..9c87709c 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/gyro_bias_module.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/gyro_bias_module.cpp @@ -14,8 +14,8 @@ #include "deviation_estimator/gyro_bias_module.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "deviation_estimator/utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" /** * @brief update gyroscope bias based on a given trajectory data diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp index 1fb84235..7629dd1f 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp @@ -14,8 +14,8 @@ #include "deviation_estimator/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -93,7 +93,7 @@ geometry_msgs::msg::Point integrate_position( { double t_prev = rclcpp::Time(vx_list.front().stamp).seconds(); double yaw = yaw_init; - geometry_msgs::msg::Point d_pos = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point d_pos = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); for (std::size_t i = 0; i < vx_list.size() - 1; ++i) { const double t_cur = rclcpp::Time(vx_list[i + 1].stamp).seconds(); const geometry_msgs::msg::Vector3 gyro_interpolated = @@ -118,9 +118,9 @@ geometry_msgs::msg::Vector3 calculate_error_rpy( const geometry_msgs::msg::Vector3 & gyro_bias) { const geometry_msgs::msg::Vector3 rpy_0 = - tier4_autoware_utils::getRPY(pose_list.front().pose.orientation); + autoware_universe_utils::getRPY(pose_list.front().pose.orientation); const geometry_msgs::msg::Vector3 rpy_1 = - tier4_autoware_utils::getRPY(pose_list.back().pose.orientation); + autoware_universe_utils::getRPY(pose_list.back().pose.orientation); const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); geometry_msgs::msg::Vector3 error_rpy = createVector3( diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/velocity_coef_module.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/velocity_coef_module.cpp index c8346ed7..a7e4c0c6 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/velocity_coef_module.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/velocity_coef_module.cpp @@ -14,8 +14,8 @@ #include "deviation_estimator/velocity_coef_module.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "deviation_estimator/utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" /** * @brief update speed scale factor (or velocity coefficient) based on a given trajectory data diff --git a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp index fc8bb471..b5152fd2 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "deviation_estimator/gyro_bias_module.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -53,7 +53,7 @@ TEST(DeviationEstimatorGyroBias, SmokeTestDefault) for (int i = 0; i <= ndt_rate * dt; ++i) { geometry_msgs::msg::PoseStamped pose; pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate); - pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); + pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); pose_list.push_back(pose); } diff --git a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp index 0e40c27f..4e1f494e 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "deviation_estimator/deviation_estimator.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -53,7 +53,7 @@ TEST(DeviationEstimatorGyroStddev, SmokeTestDefault) for (int i = 0; i <= ndt_rate * t_window; ++i) { geometry_msgs::msg::PoseStamped pose; pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate); - pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); + pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); pose_list.push_back(pose); } @@ -104,7 +104,7 @@ TEST(DeviationEstimatorGyroStddev, SmokeTestWithBias) for (int i = 0; i <= ndt_rate * t_window; ++i) { geometry_msgs::msg::PoseStamped pose; pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate); - pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); + pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); pose_list.push_back(pose); } diff --git a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/tier4_autoware_utils.hpp b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/autoware_universe_utils.hpp similarity index 62% rename from localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/tier4_autoware_utils.hpp rename to localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/autoware_universe_utils.hpp index a40b3112..64d30654 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/tier4_autoware_utils.hpp +++ b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/autoware_universe_utils.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DEVIATION_EVALUATOR__TIER4_AUTOWARE_UTILS_HPP_ -#define DEVIATION_EVALUATOR__TIER4_AUTOWARE_UTILS_HPP_ +#ifndef DEVIATION_EVALUATOR__AUTOWARE_UNIVERSE_UTILS_HPP_ +#define DEVIATION_EVALUATOR__AUTOWARE_UNIVERSE_UTILS_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include -// ToDo (kminoda): Replace these functions with the one from tier4_autoware_utils. +// ToDo (kminoda): Replace these functions with the one from autoware_universe_utils. // Currently these functions are declared here since this tool has to be compatible with older // version of Autoware. @@ -29,11 +29,11 @@ template bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) { // check the first point direction - const double src_yaw = tf2::getYaw(tier4_autoware_utils::getPose(src_pose).orientation); - const double pose_direction_yaw = tier4_autoware_utils::calcAzimuthAngle( - tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); - return std::fabs(tier4_autoware_utils::normalizeRadian(src_yaw - pose_direction_yaw)) < - tier4_autoware_utils::pi / 2.0; + const double src_yaw = tf2::getYaw(autoware_universe_utils::getPose(src_pose).orientation); + const double pose_direction_yaw = autoware_universe_utils::calcAzimuthAngle( + autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose)); + return std::fabs(autoware_universe_utils::normalizeRadian(src_yaw - pose_direction_yaw)) < + autoware_universe_utils::pi / 2.0; } /** @@ -47,8 +47,8 @@ template geometry_msgs::msg::Point calcInterpolatedPoint( const Point1 & src, const Point2 & dst, const double ratio) { - const auto src_point = tier4_autoware_utils::getPoint(src); - const auto dst_point = tier4_autoware_utils::getPoint(dst); + const auto src_point = autoware_universe_utils::getPoint(src); + const auto dst_point = autoware_universe_utils::getPoint(dst); tf2::Vector3 src_vec; src_vec.setX(src_point.x); @@ -92,33 +92,35 @@ geometry_msgs::msg::Pose calcInterpolatedPose( geometry_msgs::msg::Pose output_pose; output_pose.position = calcInterpolatedPoint( - tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose), + autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose), clamped_ratio); if (set_orientation_from_position_direction) { - const double input_poses_dist = tier4_autoware_utils::calcDistance2d( - tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); + const double input_poses_dist = autoware_universe_utils::calcDistance2d( + autoware_universe_utils::getPoint(src_pose), autoware_universe_utils::getPoint(dst_pose)); const bool is_driving_forward = isDrivingForward(src_pose, dst_pose); // Get orientation from interpolated point and src_pose if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) { - output_pose.orientation = tier4_autoware_utils::getPose(dst_pose).orientation; + output_pose.orientation = autoware_universe_utils::getPose(dst_pose).orientation; } else if (!is_driving_forward && clamped_ratio < 1e-6) { - output_pose.orientation = tier4_autoware_utils::getPose(src_pose).orientation; + output_pose.orientation = autoware_universe_utils::getPose(src_pose).orientation; } else { const auto & base_pose = is_driving_forward ? dst_pose : src_pose; - const double pitch = tier4_autoware_utils::calcElevationAngle( - tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); - const double yaw = tier4_autoware_utils::calcAzimuthAngle( - tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); - output_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + const double pitch = autoware_universe_utils::calcElevationAngle( + autoware_universe_utils::getPoint(output_pose), + autoware_universe_utils::getPoint(base_pose)); + const double yaw = autoware_universe_utils::calcAzimuthAngle( + autoware_universe_utils::getPoint(output_pose), + autoware_universe_utils::getPoint(base_pose)); + output_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } } else { // Get orientation by spherical linear interpolation tf2::Transform src_tf; tf2::Transform dst_tf; - tf2::fromMsg(tier4_autoware_utils::getPose(src_pose), src_tf); - tf2::fromMsg(tier4_autoware_utils::getPose(dst_pose), dst_tf); + tf2::fromMsg(autoware_universe_utils::getPose(src_pose), src_tf); + tf2::fromMsg(autoware_universe_utils::getPose(dst_pose), dst_tf); const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio); output_pose.orientation = tf2::toMsg(quaternion); } @@ -126,4 +128,4 @@ geometry_msgs::msg::Pose calcInterpolatedPose( return output_pose; } -#endif // DEVIATION_EVALUATOR__TIER4_AUTOWARE_UTILS_HPP_ +#endif // DEVIATION_EVALUATOR__AUTOWARE_UNIVERSE_UTILS_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp index b148e6cc..3aeace1e 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp +++ b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp @@ -15,10 +15,10 @@ #ifndef DEVIATION_EVALUATOR__DEVIATION_EVALUATOR_HPP_ #define DEVIATION_EVALUATOR__DEVIATION_EVALUATOR_HPP_ -#include "deviation_evaluator/tier4_autoware_utils.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "deviation_evaluator/autoware_universe_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" @@ -81,7 +81,7 @@ class DeviationEvaluator : public rclcpp::Node PoseStamped::SharedPtr current_ekf_gt_pose_ptr_; PoseStamped::SharedPtr current_ndt_pose_ptr_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; bool has_published_initial_pose_; diff --git a/localization/deviation_estimation_tools/deviation_evaluator/package.xml b/localization/deviation_estimation_tools/deviation_evaluator/package.xml index 7f28543d..f9be3569 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/package.xml +++ b/localization/deviation_estimation_tools/deviation_evaluator/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils geometry_msgs nav_msgs rclcpp @@ -19,7 +20,6 @@ std_srvs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_gtest diff --git a/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp b/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp index 3fa24802..3ff5541d 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp +++ b/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp @@ -16,8 +16,8 @@ #include "deviation_evaluator/deviation_evaluator.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "rclcpp/logging.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -106,7 +106,7 @@ DeviationEvaluator::DeviationEvaluator( pub_init_pose_with_cov_ = create_publisher( "out_initial_pose_with_covariance", 1); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); current_ndt_pose_ptr_ = nullptr; has_published_initial_pose_ = false; diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index 72ce45cf..362094f1 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -15,11 +15,11 @@ #ifndef PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ #define PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "planning_debug_tools/msg/trajectory_debug_info.hpp" #include "planning_debug_tools/util.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index 99230de9..05264645 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -15,9 +15,9 @@ #ifndef PLANNING_DEBUG_TOOLS__UTIL_HPP_ #define PLANNING_DEBUG_TOOLS__UTIL_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -30,9 +30,9 @@ namespace planning_debug_tools using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getPoint; -using tier4_autoware_utils::getRPY; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::getPoint; +using autoware_universe_utils::getRPY; using tier4_planning_msgs::msg::PathPointWithLaneId; double getVelocity(const PathPoint & p) @@ -88,7 +88,7 @@ inline std::vector getAccelerationArray(const T & points) const auto & prev_point = points.at(i); const auto & next_point = points.at(i + 1); - const double delta_s = tier4_autoware_utils::calcDistance2d(prev_point, next_point); + const double delta_s = autoware_universe_utils::calcDistance2d(prev_point, next_point); if (delta_s == 0.0) { segment_wise_a_arr.push_back(0.0); } else { diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index 010e9325..4f8f4cab 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -18,12 +18,12 @@ rosidl_default_generators autoware_planning_msgs + autoware_universe_utils geometry_msgs motion_utils nav_msgs rclcpp rclcpp_components - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs diff --git a/planning/planning_debug_tools/src/stop_reason_visualizer.cpp b/planning/planning_debug_tools/src/stop_reason_visualizer.cpp index 61e54f47..d59833de 100644 --- a/planning/planning_debug_tools/src/stop_reason_visualizer.cpp +++ b/planning/planning_debug_tools/src/stop_reason_visualizer.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include @@ -49,10 +49,10 @@ class StopReasonVisualizerNode : public rclcpp::Node private: void onStopReasonArray(const StopReasonArray::ConstSharedPtr msg) { - using tier4_autoware_utils::appendMarkerArray; - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; + using autoware_universe_utils::appendMarkerArray; + using autoware_universe_utils::createDefaultMarker; + using autoware_universe_utils::createMarkerColor; + using autoware_universe_utils::createMarkerScale; MarkerArray all_marker_array; const auto header = msg->header; diff --git a/vehicle/pitch_checker/include/pitch_checker/pitch_checker.hpp b/vehicle/pitch_checker/include/pitch_checker/pitch_checker.hpp index f076fab9..6d27f1a4 100644 --- a/vehicle/pitch_checker/include/pitch_checker/pitch_checker.hpp +++ b/vehicle/pitch_checker/include/pitch_checker/pitch_checker.hpp @@ -35,7 +35,7 @@ #else #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "tier4_autoware_utils/ros/transform_listener.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" struct TfInfo { @@ -52,7 +52,7 @@ class PitchChecker : public rclcpp::Node explicit PitchChecker(const rclcpp::NodeOptions & node_options); private: - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/vehicle/pitch_checker/package.xml b/vehicle/pitch_checker/package.xml index 434db35d..74ec2f72 100644 --- a/vehicle/pitch_checker/package.xml +++ b/vehicle/pitch_checker/package.xml @@ -11,6 +11,7 @@ autoware_cmake + autoware_universe_utils geometry_msgs rclcpp std_msgs @@ -18,7 +19,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils python3-matplotlib python3-pandas diff --git a/vehicle/pitch_checker/src/pitch_checker_node.cpp b/vehicle/pitch_checker/src/pitch_checker_node.cpp index 66a9a016..2487bf29 100644 --- a/vehicle/pitch_checker/src/pitch_checker_node.cpp +++ b/vehicle/pitch_checker/src/pitch_checker_node.cpp @@ -24,7 +24,7 @@ PitchChecker::PitchChecker(const rclcpp::NodeOptions & node_options) : Node("pitch_checker", node_options) { - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3;