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;