diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6fdd04df3eb79..77adc8e6479bb 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -30,7 +30,7 @@ common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp diff --git a/common/.pages b/common/.pages index 1084ec0356336..60300879035d8 100644 --- a/common/.pages +++ b/common/.pages @@ -25,7 +25,7 @@ nav: - 'Introduction': common/signal_processing - 'Butterworth Filter': common/signal_processing/documentation/ButterworthFilter - 'TensorRT Common': common/tensorrt_common - - 'tier4_autoware_utils': common/tier4_autoware_utils + - 'autoware_universe_utils': common/autoware_universe_utils - 'traffic_light_utils': common/traffic_light_utils - 'TVM Utility': - 'Introduction': common/tvm_utility diff --git a/common/README.md b/common/README.md index 95b8973a66b2b..6401aeab7b685 100644 --- a/common/README.md +++ b/common/README.md @@ -12,5 +12,5 @@ The Autoware.Universe Common folder consists of common and testing libraries tha Some of the commonly used libraries are: -1. `tier4_autoware_utils` +1. `autoware_universe_utils` 2. `motion_utils` diff --git a/common/autoware_grid_map_utils/package.xml b/common/autoware_grid_map_utils/package.xml index c15470168f318..d8344b08f2341 100644 --- a/common/autoware_grid_map_utils/package.xml +++ b/common/autoware_grid_map_utils/package.xml @@ -10,10 +10,10 @@ autoware_cmake eigen3_cmake_module + autoware_universe_utils grid_map_core grid_map_cv libopencv-dev - tier4_autoware_utils ament_cmake_gtest ament_lint_auto diff --git a/common/autoware_grid_map_utils/test/benchmark.cpp b/common/autoware_grid_map_utils/test/benchmark.cpp index 77da9ae589b73..90545135b3e18 100644 --- a/common/autoware_grid_map_utils/test/benchmark.cpp +++ b/common/autoware_grid_map_utils/test/benchmark.cpp @@ -17,13 +17,13 @@ #include "grid_map_cv/GridMapCvConverter.hpp" #include "grid_map_cv/GridMapCvProcessing.hpp" +#include #include #include #include #include #include #include -#include #include #include @@ -46,7 +46,7 @@ int main(int argc, char * argv[]) result_file << "#Size PolygonVertices PolygonIndexes grid_map_utils_constructor grid_map_utils_iteration " "grid_map_constructor grid_map_iteration\n"; - tier4_autoware_utils::StopWatch stopwatch; + autoware_universe_utils::StopWatch stopwatch; constexpr auto nb_iterations = 10; constexpr auto polygon_side_vertices = diff --git a/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp index 8d9cff560b5b7..1646586853800 100644 --- a/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp +++ b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp @@ -14,8 +14,8 @@ #include "autoware_grid_map_utils/polygon_iterator.hpp" +#include #include -#include // gtest #include diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index 093449176b047..b4b5834f71fa1 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ #define AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ +#include #include #include #include #include #include -#include #include #include @@ -62,6 +62,8 @@ using autoware_planning_msgs::msg::Trajectory; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromRPY; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -70,8 +72,6 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; using tier4_planning_msgs::msg::Scenario; using unique_identifier_msgs::msg::UUID; diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index 555345c1d9105..778b9a42ca207 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -19,6 +19,7 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -29,7 +30,6 @@ tf2_msgs tf2_ros tier4_api_msgs - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt similarity index 69% rename from common/tier4_autoware_utils/CMakeLists.txt rename to common/autoware_universe_utils/CMakeLists.txt index 9cb54e52362a5..9e86ddeb60692 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(tier4_autoware_utils) +project(autoware_universe_utils) find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -ament_auto_add_library(tier4_autoware_utils SHARED +ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp src/geometry/boost_polygon_utils.cpp @@ -23,10 +23,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(test_tier4_autoware_utils ${test_files}) + ament_add_ros_isolated_gtest(test_autoware_universe_utils ${test_files}) - target_link_libraries(test_tier4_autoware_utils - tier4_autoware_utils + target_link_libraries(test_autoware_universe_utils + autoware_universe_utils ) endif() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md new file mode 100644 index 0000000000000..22b9355b09635 --- /dev/null +++ b/common/autoware_universe_utils/README.md @@ -0,0 +1,9 @@ +# autoware_universe_utils + +## Purpose + +This package contains many common functions used by other packages, so please refer to them as needed. + +## For developers + +`autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp similarity index 84% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp index d986d0f23fc85..c90cc714367f1 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ #include #include @@ -25,7 +25,7 @@ #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { // 2D struct Point2d; @@ -93,12 +93,12 @@ inline Point3d fromMsg(const geometry_msgs::msg::Point & msg) point.z() = msg.z; return point; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT - tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT -BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT - tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT -BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT + autoware_universe_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT + autoware_universe_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT +BOOST_GEOMETRY_REGISTER_RING(autoware_universe_utils::LinearRing2d) // NOLINT -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp similarity index 85% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp index 665e959ed2460..477a1b4c27a6c 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include @@ -24,7 +24,7 @@ #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { bool isClockwise(const Polygon2d & polygon); Polygon2d inverseClockwise(const Polygon2d & polygon); @@ -47,6 +47,6 @@ Polygon2d toFootprint( const double base_to_rear, const double width); double getArea(const autoware_perception_msgs::msg::Shape & shape); Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp similarity index 97% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp index 334df8f32af91..0af9d30ad09c1 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -96,7 +96,7 @@ inline void doTransform( #endif } // namespace tf2 -namespace tier4_autoware_utils +namespace autoware_universe_utils { template geometry_msgs::msg::Point getPoint(const T & p) @@ -577,6 +577,6 @@ std::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp similarity index 82% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp index dfa678eaa07d9..2f2b4a5d3dfb0 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { struct PoseDeviation { @@ -39,6 +39,6 @@ double calcYawDeviation( PoseDeviation calcPoseDeviation( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp similarity index 74% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp index 984fa04016b57..2c1f9fe646723 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ -namespace tier4_autoware_utils +namespace autoware_universe_utils { constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20 constexpr double gravity = 9.80665; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp similarity index 79% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp index 1eb32f33886cf..502ed4b4f7b34 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__NORMALIZATION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__NORMALIZATION_HPP_ -#include "tier4_autoware_utils/math/constants.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { inline double normalizeDegree(const double deg, const double min_deg = -180) { @@ -45,6 +45,6 @@ inline double normalizeRadian(const double rad, const double min_rad = -pi) return value - std::copysign(2 * pi, value); } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__NORMALIZATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp similarity index 89% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp index 957a6f4e729e5..fabf67a683680 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__RANGE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__RANGE_HPP_ #include #include #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template std::vector arange(const T start, const T stop, const T step = 1) @@ -73,6 +73,6 @@ std::vector linspace(const T start, const T stop, const size_t num) return out; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__RANGE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp similarity index 76% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp index bea3b1e0ecb46..bf67a1b521b6d 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__SIN_TABLE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__SIN_TABLE_HPP_ #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { constexpr size_t sin_table_size = 32769; @@ -25,6 +25,6 @@ constexpr size_t discrete_arcs_num_90 = 32768; constexpr size_t discrete_arcs_num_360 = 131072; extern const float g_sin_table[sin_table_size]; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__SIN_TABLE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp similarity index 74% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp index 4901e28472acb..7f58735d35963 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { float sin(float radian); @@ -26,6 +26,6 @@ float cos(float radian); std::pair sin_and_cos(float radian); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp similarity index 74% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp index 38a0a7696775a..66be4d99d5803 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#include "tier4_autoware_utils/math/constants.hpp" +#include "autoware/universe_utils/math/constants.hpp" -namespace tier4_autoware_utils +namespace autoware_universe_utils { constexpr double deg2rad(const double deg) { @@ -36,6 +36,6 @@ constexpr double mps2kmph(const double mps) { return mps * 3600.0 / 1000.0; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp similarity index 83% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp index 0750b6894debe..0965f8ffbaefa 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ -#include "tier4_autoware_utils/ros/debug_traits.hpp" +#include "autoware/universe_utils/ros/debug_traits.hpp" #include #include @@ -25,14 +25,15 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { namespace debug_publisher { template < class T_msg, class T, std::enable_if_t< - tier4_autoware_utils::debug_traits::is_debug_message::value, std::nullptr_t> = nullptr> + autoware_universe_utils::debug_traits::is_debug_message::value, std::nullptr_t> = + nullptr> T_msg toDebugMsg(const T & data, const rclcpp::Time & stamp) { T_msg msg; @@ -72,6 +73,6 @@ class DebugPublisher const char * ns_; std::unordered_map> pub_map_; }; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp similarity index 89% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp index 345e49079eede..330b01b68a0b2 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ #include #include @@ -28,7 +28,7 @@ #include -namespace tier4_autoware_utils::debug_traits +namespace autoware_universe_utils::debug_traits { template struct is_debug_message : std::false_type @@ -84,6 +84,6 @@ template <> struct is_debug_message : std::true_type { }; -} // namespace tier4_autoware_utils::debug_traits +} // namespace autoware_universe_utils::debug_traits -#endif // TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp similarity index 80% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp index 5aee3a251dad2..41163ea38b018 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp @@ -22,12 +22,12 @@ // =============== How to use =============== // ___In your_node.hpp___ -// #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +// #include "autoware/universe_utils/ros/logger_level_configure.hpp" // class YourNode : public rclcpp::Node { // ... // // // Define logger_configure as a node class member variable -// std::unique_ptr logger_configure_; +// std::unique_ptr logger_configure_; // } // // ___In your_node.cpp___ @@ -38,14 +38,14 @@ // logger_configure_ = std::make_unique(this); // } -#ifndef TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ #include "logging_demo/srv/config_logger.hpp" #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { class LoggerLevelConfigure { @@ -64,5 +64,5 @@ class LoggerLevelConfigure const ConfigLogger::Response::SharedPtr response); }; -} // namespace tier4_autoware_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +} // namespace autoware_universe_utils +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp similarity index 89% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp index c00c3c1c364df..ad2d2f84f82e0 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ #include @@ -22,7 +22,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) { @@ -76,6 +76,6 @@ void appendMarkerArray( visualization_msgs::msg::MarkerArray * marker_array, const std::optional & current_time = {}); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp similarity index 90% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp index 4ebf81d4bfda5..55caf1955b6ec 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ -namespace tier4_autoware_utils +namespace autoware_universe_utils { namespace xyz_covariance_index { @@ -115,6 +115,6 @@ enum XYZ_UPPER_COV_IDX { Z_Z = 5, }; } // namespace xyz_upper_covariance_index -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_operation.hpp similarity index 79% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_operation.hpp index 4ac5dc05204f9..5ac3441dd2fc4 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_operation.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MSG_OPERATION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__MSG_OPERATION_HPP_ #include "geometry_msgs/msg/quaternion.hpp" -// NOTE: Do not use tier4_autoware_utils namespace +// NOTE: Do not use autoware_universe_utils namespace namespace geometry_msgs { namespace msg @@ -28,4 +28,4 @@ Quaternion operator-(Quaternion a, Quaternion b) noexcept; } // namespace msg } // namespace geometry_msgs -#endif // TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__MSG_OPERATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp similarity index 78% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp index ac9b124a02cdb..5ef1837e0cece 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PARAMETER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PARAMETER_HPP_ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) @@ -30,6 +30,6 @@ T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) return node.declare_parameter(name); } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PARAMETER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/pcl_conversion.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp similarity index 90% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/pcl_conversion.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp index c66a63eb1ac51..cce8a75b37aa9 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/pcl_conversion.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PCL_CONVERSION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PCL_CONVERSION_HPP_ #include #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { /** * @brief a faster implementation of converting sensor_msgs::msg::PointCloud2 to @@ -67,6 +67,6 @@ void transformPointCloudFromROSMsg( } } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PCL_CONVERSION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp similarity index 95% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp index be5104b489db6..a41e13229a826 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { inline rclcpp::SensorDataQoS SingleDepthSensorQoS() @@ -159,6 +159,6 @@ class InterProcessPollingSubscriber= 2)>::typ }; }; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp similarity index 86% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp index 80d394a1c200e..bb92d615b0a8d 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { class ProcessingTimePublisher { @@ -62,6 +62,6 @@ class ProcessingTimePublisher return oss.str(); } }; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp similarity index 92% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp index 21705cab9a962..960295944a14b 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { class PublishedTimePublisher @@ -109,6 +109,6 @@ class PublishedTimePublisher // store them for each different publisher of the node std::map::SharedPtr, GidCompare> publishers_; }; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp similarity index 77% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp index 8148392ecda3b..0bd9a4db5b674 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { class SelfPoseListener { @@ -53,6 +53,6 @@ class SelfPoseListener private: TransformListener transform_listener_; }; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp similarity index 91% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp index 96b1cc6ea6ccc..176a11dcde529 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { class TransformListener { @@ -82,6 +82,6 @@ class TransformListener std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; }; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp similarity index 80% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp index 36abcc11175e1..8bb505d765aef 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__UPDATE_PARAM_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__UPDATE_PARAM_HPP_ #include #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template bool updateParam(const std::vector & params, const std::string & name, T & value) @@ -37,6 +37,6 @@ bool updateParam(const std::vector & params, const std::strin value = itr->template get_value(); return true; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__UPDATE_PARAM_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp similarity index 87% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp index efb5564bdaa71..7d4892f724848 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__UUID_HELPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__UUID_HELPER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { inline unique_identifier_msgs::msg::UUID generateUUID() { @@ -58,6 +58,6 @@ inline unique_identifier_msgs::msg::UUID toUUIDMsg(const boost::uuids::uuid & id return ros_uuid; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__UUID_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp similarity index 84% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp index 67a1249c5b042..0aaef92835653 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ #include @@ -21,7 +21,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template T waitForParam( @@ -47,6 +47,6 @@ T waitForParam( return {}; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp similarity index 71% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp index c784d71ad5060..f195cd2b32391 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ -#define TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ -namespace tier4_autoware_utils +namespace autoware_universe_utils { void print_backtrace(); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp similarity index 87% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp index 0d804e797936c..3df651825f1aa 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ -#define TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__STOP_WATCH_HPP_ #include #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template < class OutputUnit = std::chrono::seconds, class InternalUnit = std::chrono::microseconds, @@ -58,6 +58,6 @@ class StopWatch std::unordered_map t_start_; }; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__STOP_WATCH_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp similarity index 85% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp index ea88ea7624216..f480a709d169e 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ -#define TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__TRANSFORM__TRANSFORMS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__TRANSFORM__TRANSFORMS_HPP_ #include #include @@ -21,7 +21,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template void transformPointCloud( @@ -46,6 +46,6 @@ void transformPointCloud( pcl::transformPointCloud(cloud_in, cloud_out, transform); } } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils -#endif // TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__TRANSFORM__TRANSFORMS_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/autoware_universe_utils/package.xml similarity index 93% rename from common/tier4_autoware_utils/package.xml rename to common/autoware_universe_utils/package.xml index 334ee226a5f22..de0461b5a841d 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/autoware_universe_utils/package.xml @@ -1,9 +1,9 @@ - tier4_autoware_utils + autoware_universe_utils 0.1.0 - The tier4_autoware_utils package + The autoware_universe_utils package Takamasa Horibe Takayuki Murooka Mamoru Sobue diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp similarity index 85% rename from common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp rename to common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp index 01e02d1cf0038..ce9232a8c1add 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -23,8 +23,8 @@ namespace { namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -68,7 +68,7 @@ double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) } } // namespace -namespace tier4_autoware_utils +namespace autoware_universe_utils { bool isClockwise(const Polygon2d & polygon) { @@ -123,16 +123,16 @@ Polygon2d toPolygon2d( Polygon2d polygon; if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - const auto point0 = tier4_autoware_utils::calcOffsetPose( + const auto point0 = autoware_universe_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; - const auto point1 = tier4_autoware_utils::calcOffsetPose( + const auto point1 = autoware_universe_utils::calcOffsetPose( pose, -shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; - const auto point2 = tier4_autoware_utils::calcOffsetPose( + const auto point2 = autoware_universe_utils::calcOffsetPose( pose, -shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) .position; - const auto point3 = tier4_autoware_utils::calcOffsetPose( + const auto point3 = autoware_universe_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) .position; @@ -168,7 +168,7 @@ Polygon2d toPolygon2d( appendPointToPolygon(polygon, abs_point); } } else { - throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); + throw std::logic_error("The shape type is not supported in autoware_universe_utils."); } // NOTE: push back the first point in order to close polygon @@ -179,24 +179,24 @@ Polygon2d toPolygon2d( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } -tier4_autoware_utils::Polygon2d toPolygon2d( +autoware_universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::DetectedObject & object) { - return tier4_autoware_utils::toPolygon2d( + return autoware_universe_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } -tier4_autoware_utils::Polygon2d toPolygon2d( +autoware_universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::TrackedObject & object) { - return tier4_autoware_utils::toPolygon2d( + return autoware_universe_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } -tier4_autoware_utils::Polygon2d toPolygon2d( +autoware_universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::PredictedObject & object) { - return tier4_autoware_utils::toPolygon2d( + return autoware_universe_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); } @@ -206,13 +206,17 @@ Polygon2d toFootprint( { Polygon2d polygon; const auto point0 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, base_to_front, width / 2.0, 0.0).position; + autoware_universe_utils::calcOffsetPose(base_link_pose, base_to_front, width / 2.0, 0.0) + .position; const auto point1 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, base_to_front, -width / 2.0, 0.0).position; + autoware_universe_utils::calcOffsetPose(base_link_pose, base_to_front, -width / 2.0, 0.0) + .position; const auto point2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -width / 2.0, 0.0).position; + autoware_universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, -width / 2.0, 0.0) + .position; const auto point3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, width / 2.0, 0.0).position; + autoware_universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, width / 2.0, 0.0) + .position; appendPointToPolygon(polygon, point0); appendPointToPolygon(polygon, point1); @@ -233,7 +237,7 @@ double getArea(const autoware_perception_msgs::msg::Shape & shape) return getPolygonArea(shape.footprint); } - throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); + throw std::logic_error("The shape type is not supported in autoware_universe_utils."); } // NOTE: The number of vertices on the expanded polygon by boost::geometry::buffer @@ -268,4 +272,4 @@ Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset) boost::geometry::correct(expanded_polygon); return expanded_polygon; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/autoware_universe_utils/src/geometry/geometry.cpp similarity index 98% rename from common/tier4_autoware_utils/src/geometry/geometry.cpp rename to common/autoware_universe_utils/src/geometry/geometry.cpp index b88646b73dd7c..e6a491671595a 100644 --- a/common/tier4_autoware_utils/src/geometry/geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -32,7 +32,7 @@ void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped().x(point32.x).y(point32.y).z(point32.z); - const auto transformed_point = tier4_autoware_utils::transformPoint(point, pose); + const auto transformed_point = autoware_universe_utils::transformPoint(point, pose); return geometry_msgs::build() .x(transformed_point.x) .y(transformed_point.y) @@ -383,4 +383,4 @@ std::optional intersect( return intersect_point; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp b/common/autoware_universe_utils/src/geometry/pose_deviation.cpp similarity index 92% rename from common/tier4_autoware_utils/src/geometry/pose_deviation.cpp rename to common/autoware_universe_utils/src/geometry/pose_deviation.cpp index dda8afb974d26..5c81519510c8e 100644 --- a/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp +++ b/common/autoware_universe_utils/src/geometry/pose_deviation.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include @@ -28,7 +28,7 @@ #include #endif -namespace tier4_autoware_utils +namespace autoware_universe_utils { double calcLateralDeviation( @@ -82,4 +82,4 @@ PoseDeviation calcPoseDeviation( return deviation; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/src/math/sin_table.cpp b/common/autoware_universe_utils/src/math/sin_table.cpp similarity index 99% rename from common/tier4_autoware_utils/src/math/sin_table.cpp rename to common/autoware_universe_utils/src/math/sin_table.cpp index 0657c2303226f..bc0a5d7b09024 100644 --- a/common/tier4_autoware_utils/src/math/sin_table.cpp +++ b/common/autoware_universe_utils/src/math/sin_table.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/sin_table.hpp" +#include "autoware/universe_utils/math/sin_table.hpp" -namespace tier4_autoware_utils +namespace autoware_universe_utils { const float g_sin_table[sin_table_size] = { @@ -8212,4 +8212,4 @@ const float g_sin_table[sin_table_size] = { 0.9999999816164293f, 0.9999999896592414f, 0.9999999954041073f, 0.9999999988510269f, 1.0000000000000000f}; -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp similarity index 81% rename from common/tier4_autoware_utils/src/math/trigonometry.cpp rename to common/autoware_universe_utils/src/math/trigonometry.cpp index 0ce65c7aa5bc8..40a993343b1de 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/autoware_universe_utils/src/math/trigonometry.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/trigonometry.hpp" +#include "autoware/universe_utils/math/trigonometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/sin_table.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/sin_table.hpp" #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { float sin(float radian) { - float degree = radian * (180.f / static_cast(tier4_autoware_utils::pi)) * + float degree = radian * (180.f / static_cast(autoware_universe_utils::pi)) * (discrete_arcs_num_360 / 360.f); size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % @@ -46,13 +46,13 @@ float sin(float radian) float cos(float radian) { - return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); + return sin(radian + static_cast(autoware_universe_utils::pi) / 2.f); } std::pair sin_and_cos(float radian) { constexpr float tmp = - (180.f / static_cast(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + (180.f / static_cast(autoware_universe_utils::pi)) * (discrete_arcs_num_360 / 360.f); const float degree = radian * tmp; size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % @@ -72,4 +72,4 @@ std::pair sin_and_cos(float radian) } } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp b/common/autoware_universe_utils/src/ros/logger_level_configure.cpp similarity index 93% rename from common/tier4_autoware_utils/src/ros/logger_level_configure.cpp rename to common/autoware_universe_utils/src/ros/logger_level_configure.cpp index d764299290d05..77e8d9bf16d7f 100644 --- a/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp +++ b/common/autoware_universe_utils/src/ros/logger_level_configure.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { LoggerLevelConfigure::LoggerLevelConfigure(rclcpp::Node * node) : ros_logger_(node->get_logger()) { @@ -58,4 +58,4 @@ void LoggerLevelConfigure::onLoggerConfigService( return; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp similarity index 94% rename from common/tier4_autoware_utils/src/ros/marker_helper.cpp rename to common/autoware_universe_utils/src/ros/marker_helper.cpp index fda997edc83db..d4f97af2ef90f 100644 --- a/common/tier4_autoware_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" -namespace tier4_autoware_utils +namespace autoware_universe_utils { visualization_msgs::msg::Marker createDefaultMarker( @@ -68,4 +68,4 @@ void appendMarkerArray( } } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/src/ros/msg_operation.cpp b/common/autoware_universe_utils/src/ros/msg_operation.cpp similarity index 92% rename from common/tier4_autoware_utils/src/ros/msg_operation.cpp rename to common/autoware_universe_utils/src/ros/msg_operation.cpp index cc1a59317a8e0..02c24b2a6cb33 100644 --- a/common/tier4_autoware_utils/src/ros/msg_operation.cpp +++ b/common/autoware_universe_utils/src/ros/msg_operation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/ros/msg_operation.hpp" +#include "autoware/universe_utils/ros/msg_operation.hpp" #include @@ -22,7 +22,7 @@ #include #endif -// NOTE: Do not use tier4_autoware_utils namespace +// NOTE: Do not use autoware_universe_utils namespace namespace geometry_msgs { namespace msg diff --git a/common/tier4_autoware_utils/src/system/backtrace.cpp b/common/autoware_universe_utils/src/system/backtrace.cpp similarity index 85% rename from common/tier4_autoware_utils/src/system/backtrace.cpp rename to common/autoware_universe_utils/src/system/backtrace.cpp index 343f200296cad..f4c4312ce44fb 100644 --- a/common/tier4_autoware_utils/src/system/backtrace.cpp +++ b/common/autoware_universe_utils/src/system/backtrace.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/system/backtrace.hpp" +#include "autoware/universe_utils/system/backtrace.hpp" #include "rclcpp/rclcpp.hpp" @@ -23,7 +23,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { void print_backtrace() @@ -44,9 +44,9 @@ void print_backtrace() for (int i = 1; i < addrlen; i++) { ss << " @ " << symbol_list[i] << std::endl; } - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("tier4_autoware_utils"), ss.str()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("autoware_universe_utils"), ss.str()); free(symbol_list); } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp similarity index 88% rename from common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp rename to common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp index 4e5483935da00..764bb388fc74a 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include @@ -20,8 +20,8 @@ namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Point3d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Point3d; TEST(boost_geometry, boost_geometry_distance) { @@ -54,7 +54,7 @@ TEST(boost_geometry, to_2d) TEST(boost_geometry, toMsg) { - using tier4_autoware_utils::toMsg; + using autoware_universe_utils::toMsg; { const Point3d p(1.0, 2.0, 3.0); @@ -68,7 +68,7 @@ TEST(boost_geometry, toMsg) TEST(boost_geometry, fromMsg) { - using tier4_autoware_utils::fromMsg; + using autoware_universe_utils::fromMsg; geometry_msgs::msg::Point p_msg; p_msg.x = 1.0; diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp similarity index 95% rename from common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp rename to common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp index 7689544bd79cc..ed576ba0dceaf 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Polygon2d; namespace { @@ -39,7 +39,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double p.position.x = x; p.position.y = y; p.position.z = 0.0; - p.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + p.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); return p; } @@ -47,7 +47,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(boost_geometry, boost_isClockwise) { - using tier4_autoware_utils::isClockwise; + using autoware_universe_utils::isClockwise; // empty Polygon2d empty_polygon; @@ -72,8 +72,8 @@ TEST(boost_geometry, boost_isClockwise) TEST(boost_geometry, boost_inverseClockwise) { - using tier4_autoware_utils::inverseClockwise; - using tier4_autoware_utils::isClockwise; + using autoware_universe_utils::inverseClockwise; + using autoware_universe_utils::isClockwise; // empty Polygon2d empty_polygon; @@ -100,7 +100,7 @@ TEST(boost_geometry, boost_inverseClockwise) TEST(boost_geometry, boost_rotatePolygon) { constexpr double epsilon = 1e-6; - using tier4_autoware_utils::rotatePolygon; + using autoware_universe_utils::rotatePolygon; // empty geometry_msgs::msg::Polygon empty_polygon; @@ -130,7 +130,7 @@ TEST(boost_geometry, boost_rotatePolygon) TEST(boost_geometry, boost_toPolygon2d) { - using tier4_autoware_utils::toPolygon2d; + using autoware_universe_utils::toPolygon2d; { // bounding box const double x = 1.0; @@ -206,7 +206,7 @@ TEST(boost_geometry, boost_toPolygon2d) TEST(boost_geometry, boost_toFootprint) { - using tier4_autoware_utils::toFootprint; + using autoware_universe_utils::toFootprint; // from base link { @@ -234,7 +234,7 @@ TEST(boost_geometry, boost_toFootprint) TEST(boost_geometry, boost_getArea) { - using tier4_autoware_utils::getArea; + using autoware_universe_utils::getArea; { // bounding box const double x = 1.0; @@ -290,7 +290,7 @@ TEST(boost_geometry, boost_getArea) TEST(boost_geometry, boost_expandPolygon) { - using tier4_autoware_utils::expandPolygon; + using autoware_universe_utils::expandPolygon; { // box with a certain offset Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}}; diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp similarity index 90% rename from common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp rename to common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index b492da2d72e88..37e43c0ce08dd 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-6; TEST(geometry, getPoint) { - using tier4_autoware_utils::getPoint; + using autoware_universe_utils::getPoint; const double x_ans = 1.0; const double y_ans = 2.0; @@ -114,7 +114,7 @@ TEST(geometry, getPoint) TEST(geometry, getPose) { - using tier4_autoware_utils::getPose; + using autoware_universe_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -203,7 +203,7 @@ TEST(geometry, getPose) TEST(geometry, getLongitudinalVelocity) { - using tier4_autoware_utils::getLongitudinalVelocity; + using autoware_universe_utils::getLongitudinalVelocity; const double velocity = 1.0; @@ -222,7 +222,7 @@ TEST(geometry, getLongitudinalVelocity) TEST(geometry, setPose) { - using tier4_autoware_utils::setPose; + using autoware_universe_utils::setPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -292,9 +292,9 @@ TEST(geometry, setPose) TEST(geometry, setOrientation) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::setOrientation; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::setOrientation; geometry_msgs::msg::Pose p; const auto orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); @@ -308,7 +308,7 @@ TEST(geometry, setOrientation) TEST(geometry, setLongitudinalVelocity) { - using tier4_autoware_utils::setLongitudinalVelocity; + using autoware_universe_utils::setLongitudinalVelocity; const double velocity = 1.0; @@ -327,7 +327,7 @@ TEST(geometry, setLongitudinalVelocity) TEST(geometry, createPoint) { - using tier4_autoware_utils::createPoint; + using autoware_universe_utils::createPoint; const geometry_msgs::msg::Point p_out = createPoint(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(p_out.x, 1.0); @@ -337,7 +337,7 @@ TEST(geometry, createPoint) TEST(geometry, createQuaternion) { - using tier4_autoware_utils::createQuaternion; + using autoware_universe_utils::createQuaternion; // (0.18257419, 0.36514837, 0.54772256, 0.73029674) is normalized quaternion of (1, 2, 3, 4) const geometry_msgs::msg::Quaternion q_out = @@ -350,7 +350,7 @@ TEST(geometry, createQuaternion) TEST(geometry, createTranslation) { - using tier4_autoware_utils::createTranslation; + using autoware_universe_utils::createTranslation; const geometry_msgs::msg::Vector3 v_out = createTranslation(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(v_out.x, 1.0); @@ -360,8 +360,8 @@ TEST(geometry, createTranslation) TEST(geometry, createQuaternionFromRPY) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; { const auto q_out = createQuaternionFromRPY(0, 0, 0); @@ -390,8 +390,8 @@ TEST(geometry, createQuaternionFromRPY) TEST(geometry, createQuaternionFromYaw) { - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::createQuaternionFromYaw; + using autoware_universe_utils::deg2rad; { const auto q_out = createQuaternionFromYaw(0); @@ -420,9 +420,9 @@ TEST(geometry, createQuaternionFromYaw) TEST(geometry, calcElevationAngle) { - using tier4_autoware_utils::calcElevationAngle; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::calcElevationAngle; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; { const auto p1 = createPoint(1.0, 1.0, 1.0); @@ -468,9 +468,9 @@ TEST(geometry, calcElevationAngle) TEST(geometry, calcAzimuthAngle) { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::calcAzimuthAngle; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; { const auto p1 = createPoint(0.0, 0.0, 9.0); @@ -521,7 +521,7 @@ TEST(geometry, calcAzimuthAngle) TEST(geometry, calcDistance2d) { - using tier4_autoware_utils::calcDistance2d; + using autoware_universe_utils::calcDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -538,7 +538,7 @@ TEST(geometry, calcDistance2d) TEST(geometry, calcSquaredDistance2d) { - using tier4_autoware_utils::calcSquaredDistance2d; + using autoware_universe_utils::calcSquaredDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -555,7 +555,7 @@ TEST(geometry, calcSquaredDistance2d) TEST(geometry, calcDistance3d) { - using tier4_autoware_utils::calcDistance3d; + using autoware_universe_utils::calcDistance3d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -572,9 +572,9 @@ TEST(geometry, calcDistance3d) TEST(geometry, getRPY) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getRPY; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getRPY; { const double ans_roll = deg2rad(5); @@ -610,9 +610,9 @@ TEST(geometry, getRPY) TEST(geometry, getRPY_wrapper) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getRPY; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getRPY; { const double ans_roll = deg2rad(45); @@ -652,9 +652,9 @@ TEST(geometry, getRPY_wrapper) TEST(geometry, transform2pose) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::transform2pose; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::transform2pose; { geometry_msgs::msg::Transform transform; @@ -703,9 +703,9 @@ TEST(geometry, transform2pose) TEST(geometry, pose2transform) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::pose2transform; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::pose2transform; { geometry_msgs::msg::Pose pose; @@ -756,9 +756,9 @@ TEST(geometry, pose2transform) TEST(geometry, point2tfVector) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::point2tfVector; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::point2tfVector; // Point { @@ -823,11 +823,11 @@ TEST(geometry, point2tfVector) TEST(geometry, transformPoint) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::Point2d; - using tier4_autoware_utils::Point3d; - using tier4_autoware_utils::transformPoint; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::Point2d; + using autoware_universe_utils::Point3d; + using autoware_universe_utils::transformPoint; { const Point2d p(1.0, 2.0); @@ -916,9 +916,9 @@ TEST(geometry, transformPoint) TEST(geometry, transformPose) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::transformPose; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::transformPose; geometry_msgs::msg::Pose pose; pose.position.x = 2.0; @@ -967,9 +967,9 @@ TEST(geometry, transformPose) TEST(geometry, inverseTransformPose) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::inverseTransformPose; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::inverseTransformPose; geometry_msgs::msg::Pose pose; pose.position.x = 2.0; @@ -1018,10 +1018,10 @@ TEST(geometry, inverseTransformPose) TEST(geometry, inverseTransformPoint) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::inverseTransformPoint; - using tier4_autoware_utils::inverseTransformPose; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::inverseTransformPoint; + using autoware_universe_utils::inverseTransformPose; geometry_msgs::msg::Pose pose_transform; pose_transform.position.x = 1.0; @@ -1050,10 +1050,10 @@ TEST(geometry, inverseTransformPoint) TEST(geometry, transformVector) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::MultiPoint3d; - using tier4_autoware_utils::transformVector; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::MultiPoint3d; + using autoware_universe_utils::transformVector; { const MultiPoint3d ps{{1.0, 2.0, 3.0}, {2.0, 3.0, 4.0}}; @@ -1078,51 +1078,51 @@ TEST(geometry, transformVector) TEST(geometry, calcCurvature) { - using tier4_autoware_utils::calcCurvature; + using autoware_universe_utils::calcCurvature; // Straight Line { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.0); } // Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -1.0); } // Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -0.2); } // Counter-Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(-1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(-2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(-1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(-2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 1.0); } // Counter-Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(-5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(-10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(-5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_universe_utils::createPoint(-10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.2); } // Give same points { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); ASSERT_ANY_THROW(calcCurvature(p1, p1, p1)); ASSERT_ANY_THROW(calcCurvature(p1, p1, p2)); ASSERT_ANY_THROW(calcCurvature(p1, p2, p1)); @@ -1132,11 +1132,11 @@ TEST(geometry, calcCurvature) TEST(geometry, calcOffsetPose) { - using tier4_autoware_utils::calcOffsetPose; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::calcOffsetPose; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::createQuaternion; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; // Only translation { @@ -1224,12 +1224,12 @@ TEST(geometry, calcOffsetPose) TEST(geometry, isDrivingForward) { - using tier4_autoware_utils::calcInterpolatedPoint; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::isDrivingForward; + using autoware_universe_utils::calcInterpolatedPoint; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::createQuaternion; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::isDrivingForward; const double epsilon = 1e-3; @@ -1286,8 +1286,8 @@ TEST(geometry, isDrivingForward) TEST(geometry, calcInterpolatedPoint) { - using tier4_autoware_utils::calcInterpolatedPoint; - using tier4_autoware_utils::createPoint; + using autoware_universe_utils::calcInterpolatedPoint; + using autoware_universe_utils::createPoint; { const auto src_point = createPoint(0.0, 0.0, 0.0); @@ -1343,11 +1343,11 @@ TEST(geometry, calcInterpolatedPoint) TEST(geometry, calcInterpolatedPose) { - using tier4_autoware_utils::calcInterpolatedPose; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::calcInterpolatedPose; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::createQuaternion; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; const double epsilon = 1e-3; // Position Interpolation @@ -1527,11 +1527,11 @@ TEST(geometry, calcInterpolatedPose) TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) { - using tier4_autoware_utils::calcInterpolatedPose; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::calcInterpolatedPose; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::createQuaternion; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; const double epsilon = 1e-3; // Position Interpolation @@ -1675,7 +1675,7 @@ TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) TEST(geometry, getTwist) { - using tier4_autoware_utils::createVector3; + using autoware_universe_utils::createVector3; geometry_msgs::msg::Vector3 velocity = createVector3(1.0, 2.0, 3.0); geometry_msgs::msg::Vector3 angular = createVector3(0.1, 0.2, 0.3); @@ -1691,7 +1691,7 @@ TEST(geometry, getTwist) // getTwist { - auto t_out = tier4_autoware_utils::createTwist(velocity, angular); + auto t_out = autoware_universe_utils::createTwist(velocity, angular); EXPECT_DOUBLE_EQ(t_out.linear.x, twist.linear.x); EXPECT_DOUBLE_EQ(t_out.linear.y, twist.linear.y); EXPECT_DOUBLE_EQ(t_out.linear.z, twist.linear.z); @@ -1703,32 +1703,32 @@ TEST(geometry, getTwist) TEST(geometry, getTwistNorm) { - using tier4_autoware_utils::createVector3; + using autoware_universe_utils::createVector3; geometry_msgs::msg::TwistWithCovariance twist_with_covariance; twist_with_covariance.twist = geometry_msgs::build() .linear(createVector3(3.0, 4.0, 0.0)) .angular(createVector3(0.1, 0.2, 0.3)); - EXPECT_NEAR(tier4_autoware_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); + EXPECT_NEAR(autoware_universe_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); } TEST(geometry, isTwistCovarianceValid) { - using tier4_autoware_utils::createVector3; + using autoware_universe_utils::createVector3; geometry_msgs::msg::TwistWithCovariance twist_with_covariance; twist_with_covariance.twist = geometry_msgs::build() .linear(createVector3(1.0, 2.0, 3.0)) .angular(createVector3(0.1, 0.2, 0.3)); - EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), false); + EXPECT_EQ(autoware_universe_utils::isTwistCovarianceValid(twist_with_covariance), false); twist_with_covariance.covariance.at(0) = 1.0; - EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), true); + EXPECT_EQ(autoware_universe_utils::isTwistCovarianceValid(twist_with_covariance), true); } TEST(geometry, intersect) { - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::intersect; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::intersect; { // Normally crossing const auto p1 = createPoint(0.0, -1.0, 0.0); diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp similarity index 92% rename from common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp rename to common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index 5c55a09af95b1..058149af2c192 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include TEST(geometry, getPoint_PathWithLaneId) { - using tier4_autoware_utils::getPoint; + using autoware_universe_utils::getPoint; const double x_ans = 1.0; const double y_ans = 2.0; @@ -36,7 +36,7 @@ TEST(geometry, getPoint_PathWithLaneId) TEST(geometry, getPose_PathWithLaneId) { - using tier4_autoware_utils::getPose; + using autoware_universe_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -66,7 +66,7 @@ TEST(geometry, getPose_PathWithLaneId) TEST(geometry, getLongitudinalVelocity_PathWithLaneId) { - using tier4_autoware_utils::getLongitudinalVelocity; + using autoware_universe_utils::getLongitudinalVelocity; const double velocity = 1.0; @@ -77,7 +77,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) TEST(geometry, setPose_PathWithLaneId) { - using tier4_autoware_utils::setPose; + using autoware_universe_utils::setPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -109,7 +109,7 @@ TEST(geometry, setPose_PathWithLaneId) TEST(geometry, setLongitudinalVelocity_PathWithLaneId) { - using tier4_autoware_utils::setLongitudinalVelocity; + using autoware_universe_utils::setLongitudinalVelocity; const double velocity = 1.0; diff --git a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp b/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp similarity index 78% rename from common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp rename to common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp index 76e742edde59c..0808223bb9ea2 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include TEST(geometry, pose_deviation) { - using tier4_autoware_utils::calcPoseDeviation; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::calcPoseDeviation; + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::deg2rad; geometry_msgs::msg::Pose pose1; pose1.position.x = 1.0; diff --git a/common/tier4_autoware_utils/test/src/math/test_constants.cpp b/common/autoware_universe_utils/test/src/math/test_constants.cpp similarity index 85% rename from common/tier4_autoware_utils/test/src/math/test_constants.cpp rename to common/autoware_universe_utils/test/src/math/test_constants.cpp index 80db5414a30f4..47e76efd66f6e 100644 --- a/common/tier4_autoware_utils/test/src/math/test_constants.cpp +++ b/common/autoware_universe_utils/test/src/math/test_constants.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/constants.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include TEST(constants, pi) { - using tier4_autoware_utils::pi; + using autoware_universe_utils::pi; EXPECT_DOUBLE_EQ(pi, 3.14159265358979323846); } TEST(constants, gravity) { - using tier4_autoware_utils::gravity; + using autoware_universe_utils::gravity; EXPECT_DOUBLE_EQ(gravity, 9.80665); } diff --git a/common/tier4_autoware_utils/test/src/math/test_normalization.cpp b/common/autoware_universe_utils/test/src/math/test_normalization.cpp similarity index 94% rename from common/tier4_autoware_utils/test/src/math/test_normalization.cpp rename to common/autoware_universe_utils/test/src/math/test_normalization.cpp index 0ae15ea320850..19b8a29019057 100644 --- a/common/tier4_autoware_utils/test/src/math/test_normalization.cpp +++ b/common/autoware_universe_utils/test/src/math/test_normalization.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include TEST(normalization, normalizeDegree) { - using tier4_autoware_utils::normalizeDegree; + using autoware_universe_utils::normalizeDegree; // -180 <= deg < 180 { @@ -51,7 +51,7 @@ TEST(normalization, normalizeDegree) TEST(normalization, normalizeRadian) { - using tier4_autoware_utils::normalizeRadian; + using autoware_universe_utils::normalizeRadian; // -M_PI <= deg < M_PI { diff --git a/common/tier4_autoware_utils/test/src/math/test_range.cpp b/common/autoware_universe_utils/test/src/math/test_range.cpp similarity index 96% rename from common/tier4_autoware_utils/test/src/math/test_range.cpp rename to common/autoware_universe_utils/test/src/math/test_range.cpp index 429c08938949f..400a6a4dd20c1 100644 --- a/common/tier4_autoware_utils/test/src/math/test_range.cpp +++ b/common/autoware_universe_utils/test/src/math/test_range.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/range.hpp" +#include "autoware/universe_utils/math/range.hpp" #include @@ -44,7 +44,7 @@ void expect_eq_vector(const std::vector & input, const std::vector & e TEST(arange_Test, arange_double) { - using tier4_autoware_utils::arange; + using autoware_universe_utils::arange; // general cases { @@ -82,7 +82,7 @@ TEST(arange_Test, arange_double) TEST(arange_Test, arange_float) { - using tier4_autoware_utils::arange; + using autoware_universe_utils::arange; // general cases { @@ -121,7 +121,7 @@ TEST(arange_Test, arange_float) TEST(arange_Test, arange_int) { - using tier4_autoware_utils::arange; + using autoware_universe_utils::arange; // general cases { @@ -154,7 +154,7 @@ TEST(arange_Test, arange_int) TEST(test_linspace, linspace_double) { - using tier4_autoware_utils::linspace; + using autoware_universe_utils::linspace; // general cases { @@ -182,7 +182,7 @@ TEST(test_linspace, linspace_double) TEST(test_linspace, linspace_float) { - using tier4_autoware_utils::linspace; + using autoware_universe_utils::linspace; // general cases { @@ -211,7 +211,7 @@ TEST(test_linspace, linspace_float) TEST(test_linspace, linspace_int) { - using tier4_autoware_utils::linspace; + using autoware_universe_utils::linspace; // general cases { diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp similarity index 69% rename from common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp rename to common/autoware_universe_utils/test/src/math/test_trigonometry.cpp index d7106fd823682..c4ba81a6b77ff 100644 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/trigonometry.hpp" #include @@ -21,31 +21,31 @@ TEST(trigonometry, sin) { - float x = 4.f * tier4_autoware_utils::pi / 128.f; + float x = 4.f * autoware_universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { EXPECT_TRUE( std::abs( std::sin(x * static_cast(i)) - - tier4_autoware_utils::sin(x * static_cast(i))) < 10e-5); + autoware_universe_utils::sin(x * static_cast(i))) < 10e-5); } } TEST(trigonometry, cos) { - float x = 4.f * tier4_autoware_utils::pi / 128.f; + float x = 4.f * autoware_universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { EXPECT_TRUE( std::abs( std::cos(x * static_cast(i)) - - tier4_autoware_utils::cos(x * static_cast(i))) < 10e-5); + autoware_universe_utils::cos(x * static_cast(i))) < 10e-5); } } TEST(trigonometry, sin_and_cos) { - float x = 4.f * tier4_autoware_utils::pi / 128.f; + float x = 4.f * autoware_universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { - const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast(i)); + const auto sin_and_cos = autoware_universe_utils::sin_and_cos(x * static_cast(i)); EXPECT_TRUE(std::abs(std::sin(x * static_cast(i)) - sin_and_cos.first) < 10e-7); EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); } diff --git a/common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp b/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp similarity index 86% rename from common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp rename to common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp index 9b689d1257932..e00b85f9e24f2 100644 --- a/common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp +++ b/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include -using tier4_autoware_utils::pi; +using autoware_universe_utils::pi; TEST(unit_conversion, deg2rad) { - using tier4_autoware_utils::deg2rad; + using autoware_universe_utils::deg2rad; EXPECT_DOUBLE_EQ(deg2rad(-720), -4 * pi); EXPECT_DOUBLE_EQ(deg2rad(0), 0); @@ -33,7 +33,7 @@ TEST(unit_conversion, deg2rad) TEST(unit_conversion, rad2deg) { - using tier4_autoware_utils::rad2deg; + using autoware_universe_utils::rad2deg; EXPECT_DOUBLE_EQ(rad2deg(-4 * pi), -720); EXPECT_DOUBLE_EQ(rad2deg(0), 0); @@ -46,7 +46,7 @@ TEST(unit_conversion, rad2deg) TEST(unit_conversion, kmph2mps) { - using tier4_autoware_utils::kmph2mps; + using autoware_universe_utils::kmph2mps; EXPECT_DOUBLE_EQ(kmph2mps(0), 0); EXPECT_DOUBLE_EQ(kmph2mps(36), 10); @@ -56,7 +56,7 @@ TEST(unit_conversion, kmph2mps) TEST(unit_conversion, mps2kmph) { - using tier4_autoware_utils::mps2kmph; + using autoware_universe_utils::mps2kmph; EXPECT_DOUBLE_EQ(mps2kmph(0), 0); EXPECT_DOUBLE_EQ(mps2kmph(10), 36); diff --git a/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp similarity index 96% rename from common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp rename to common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp index 133cb01f9b348..238a97e901611 100644 --- a/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include @@ -24,7 +24,7 @@ class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test { protected: std::shared_ptr node_{nullptr}; - std::shared_ptr published_time_publisher_ptr_{ + std::shared_ptr published_time_publisher_ptr_{ nullptr}; std::shared_ptr> first_test_publisher_ptr_{nullptr}; @@ -70,7 +70,7 @@ class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test // Create a PublishedTimePublisher published_time_publisher_ptr_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); // Create the first subscriber first_test_subscriber_ptr_ = @@ -98,7 +98,7 @@ class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test { protected: std::shared_ptr node_{nullptr}; - std::shared_ptr published_time_publisher_ptr_{ + std::shared_ptr published_time_publisher_ptr_{ nullptr}; std::shared_ptr> first_test_publisher_ptr_{nullptr}; @@ -135,7 +135,7 @@ class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test // Create a PublishedTimePublisher published_time_publisher_ptr_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); rclcpp::spin_some(node_); } diff --git a/common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp b/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp similarity index 91% rename from common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp rename to common/autoware_universe_utils/test/src/system/test_stop_watch.cpp index cb201ab0dd5ec..6bf9857595f98 100644 --- a/common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp +++ b/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include @@ -21,7 +21,7 @@ TEST(system, StopWatch_sec) { - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::StopWatch; StopWatch stop_watch; @@ -50,7 +50,7 @@ TEST(system, StopWatch_sec) TEST(system, StopWatch_msec) { - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::StopWatch; StopWatch stop_watch; diff --git a/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp b/common/autoware_universe_utils/test/src/test_autoware_utils.cpp similarity index 100% rename from common/tier4_autoware_utils/test/src/test_autoware_utils.cpp rename to common/autoware_universe_utils/test/src/test_autoware_utils.cpp diff --git a/common/tier4_autoware_utils/test/src/transform/test_transform.cpp b/common/autoware_universe_utils/test/src/transform/test_transform.cpp similarity index 85% rename from common/tier4_autoware_utils/test/src/transform/test_transform.cpp rename to common/autoware_universe_utils/test/src/transform/test_transform.cpp index aae8cc22ea9fb..e659ec0f2a749 100644 --- a/common/tier4_autoware_utils/test/src/transform/test_transform.cpp +++ b/common/autoware_universe_utils/test/src/transform/test_transform.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/transform/transforms.hpp" +#include "autoware/universe_utils/transform/transforms.hpp" #include #include @@ -31,7 +31,7 @@ TEST(system, transform_point_cloud) 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; pcl::PointCloud cloud_transformed; - tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform); + autoware_universe_utils::transformPointCloud(cloud, cloud_transformed, transform); pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4); @@ -52,8 +52,9 @@ TEST(system, empty_point_cloud) pcl::PointCloud cloud_transformed; - EXPECT_NO_THROW(tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); + EXPECT_NO_THROW( + autoware_universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); EXPECT_NO_FATAL_FAILURE( - tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); + autoware_universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); EXPECT_EQ(cloud_transformed.size(), 0ul); } diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index 3329c1349b707..370765a81c743 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -15,8 +15,8 @@ #ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ #define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ +#include #include -#include #include #include @@ -25,7 +25,7 @@ namespace goal_distance_calculator { -using tier4_autoware_utils::PoseDeviation; +using autoware_universe_utils::PoseDeviation; struct Param { diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 680c4a3cdfff1..c1154884e8340 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -17,9 +17,9 @@ #include "goal_distance_calculator/goal_distance_calculator.hpp" +#include +#include #include -#include -#include #include #include @@ -45,7 +45,7 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber rclcpp::Subscription::SharedPtr sub_initial_pose_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; + autoware_universe_utils::SelfPoseListener self_pose_listener_; rclcpp::Subscription::SharedPtr sub_route_; // Data Buffer @@ -56,7 +56,7 @@ class GoalDistanceCalculatorNode : public rclcpp::Node void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); // Publisher - tier4_autoware_utils::DebugPublisher debug_publisher_; + autoware_universe_utils::DebugPublisher debug_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index eb71dc45f31a3..04ea8a37a5053 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -11,10 +11,10 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp index 719baef283791..5491fe96c0848 100755 --- a/common/goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - tier4_autoware_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); + autoware_universe_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); return output; } diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index ab1c35e246719..7f4346dac63dc 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -14,9 +14,9 @@ #include "goal_distance_calculator/goal_distance_calculator_node.hpp" +#include #include #include -#include #include @@ -107,7 +107,7 @@ void GoalDistanceCalculatorNode::onTimer() output_ = goal_distance_calculator_->update(input_); { - using tier4_autoware_utils::rad2deg; + using autoware_universe_utils::rad2deg; const auto & deviation = output_.goal_deviation; debug_publisher_.publish( diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index f5befefeae9da..38e7b3fed5b8b 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -15,8 +15,8 @@ #ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_ #define INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/interpolation_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index 653d6c61d05b6..b2fc5c6c36e54 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -53,7 +53,7 @@ class SplineInterpolationPoints2d { std::vector points_inner; for (const auto & p : points) { - points_inner.push_back(tier4_autoware_utils::getPoint(p)); + points_inner.push_back(autoware_universe_utils::getPoint(p)); } calcSplineCoefficientsInner(points_inner); } diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index 6be04482da3ee..bb4af924dd252 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -11,7 +11,7 @@ ament_cmake_auto autoware_cmake - tier4_autoware_utils + autoware_universe_utils ament_cmake_ros ament_lint_auto diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 61c60df7a8984..11bc407e4c511 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -118,7 +118,7 @@ geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( geometry_msgs::msg::Pose pose; pose.position = getSplineInterpolatedPoint(idx, s); pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + autoware_universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); return pose; } diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index 94baf50992cf5..d3cb2d6d3060b 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.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 "interpolation/spline_interpolation.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index 09973826387a2..2d582e2318600 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-6; TEST(spline_interpolation, splineYawFromPoints) { - using tier4_autoware_utils::createPoint; + using autoware_universe_utils::createPoint; { // straight std::vector points; @@ -96,7 +96,7 @@ TEST(spline_interpolation, splineYawFromPoints) TEST(spline_interpolation, SplineInterpolationPoints2d) { - using tier4_autoware_utils::createPoint; + using autoware_universe_utils::createPoint; // curve std::vector points; @@ -200,7 +200,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { using autoware_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::createPoint; + using autoware_universe_utils::createPoint; std::vector points; points.push_back(createPoint(-2.0, -10.0, 0.0)); diff --git a/common/motion_utils/docs/vehicle/vehicle.md b/common/motion_utils/docs/vehicle/vehicle.md index c768e6115731c..32cbf6be28dcc 100644 --- a/common/motion_utils/docs/vehicle/vehicle.md +++ b/common/motion_utils/docs/vehicle/vehicle.md @@ -36,7 +36,7 @@ bool isVehicleStopped(const double stop_duration) Necessary includes: ```c++ -#include +#include ``` 1.Create a checker instance. @@ -116,7 +116,7 @@ bool isVehicleStoppedAtStopPoint(const double stop_duration) Necessary includes: ```c++ -#include +#include ``` 1.Create a checker instance. diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index fb151ec56b085..ad312701faad0 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -15,11 +15,11 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ -#include "tier4_autoware_utils/system/backtrace.hpp" +#include "autoware/universe_utils/system/backtrace.hpp" +#include #include #include -#include #include @@ -50,9 +50,9 @@ template bool validate_points_duplication(const T & points) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pt = tier4_autoware_utils::getPoint(points.at(i)); - const auto & next_pt = tier4_autoware_utils::getPoint(points.at(i + 1)); - const double ds = tier4_autoware_utils::calcDistance2d(curr_pt, next_pt); + const auto & curr_pt = autoware_universe_utils::getPoint(points.at(i)); + const auto & next_pt = autoware_universe_utils::getPoint(points.at(i + 1)); + const double ds = autoware_universe_utils::calcDistance2d(curr_pt, next_pt); if (ds < close_s_threshold) { return false; } @@ -67,27 +67,27 @@ bool validate_arguments(const T & input_points, const std::vector & resa // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { RCLCPP_DEBUG( get_logger(), "invalid argument: The number of resampling intervals is less than 2"); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points"); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); return false; } @@ -100,7 +100,7 @@ bool validate_arguments(const T & input_points, const double resampling_interval // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); return false; } @@ -109,14 +109,14 @@ bool validate_arguments(const T & input_points, const double resampling_interval RCLCPP_DEBUG( get_logger(), "invalid argument: resampling interval is less than %f", motion_utils::overlap_threshold); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index 0a0b14cb7488d..86a8048b9eec4 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -15,7 +15,7 @@ #ifndef MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ #define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" @@ -73,22 +73,22 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar } if (points.size() < 2 || target_length < 0.0) { - return tier4_autoware_utils::getPose(points.front()); + return autoware_universe_utils::getPose(points.front()); } double accumulated_length = 0; for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pose = tier4_autoware_utils::getPose(points.at(i)); - const auto & next_pose = tier4_autoware_utils::getPose(points.at(i + 1)); - const double length = tier4_autoware_utils::calcDistance3d(curr_pose, next_pose); + const auto & curr_pose = autoware_universe_utils::getPose(points.at(i)); + const auto & next_pose = autoware_universe_utils::getPose(points.at(i + 1)); + const double length = autoware_universe_utils::calcDistance3d(curr_pose, next_pose); if (accumulated_length + length > target_length) { const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6); - return tier4_autoware_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); + return autoware_universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); } accumulated_length += length; } - return tier4_autoware_utils::getPose(points.back()); + return autoware_universe_utils::getPose(points.back()); } } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 3715f466e7684..d9cb853d7db92 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -15,10 +15,10 @@ #ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ #define MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/system/backtrace.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/system/backtrace.hpp" #include #include @@ -51,7 +51,7 @@ template void validateNonEmpty(const T & points) { if (points.empty()) { - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); } } @@ -72,22 +72,22 @@ extern template void validateNonEmpty void validateNonSharpAngle( const T & point1, const T & point2, const T & point3, - const double angle_threshold = tier4_autoware_utils::pi / 4) + const double angle_threshold = autoware_universe_utils::pi / 4) { - const auto p1 = tier4_autoware_utils::getPoint(point1); - const auto p2 = tier4_autoware_utils::getPoint(point2); - const auto p3 = tier4_autoware_utils::getPoint(point3); + const auto p1 = autoware_universe_utils::getPoint(point1); + const auto p2 = autoware_universe_utils::getPoint(point2); + const auto p3 = autoware_universe_utils::getPoint(point3); const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware_universe_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = autoware_universe_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -105,10 +105,10 @@ std::optional isDrivingForward(const T & points) } // check the first point direction - const auto & first_pose = tier4_autoware_utils::getPose(points.at(0)); - const auto & second_pose = tier4_autoware_utils::getPose(points.at(1)); + const auto & first_pose = autoware_universe_utils::getPose(points.at(0)); + const auto & second_pose = autoware_universe_utils::getPose(points.at(1)); - return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); + return autoware_universe_utils::isDrivingForward(first_pose, second_pose); } extern template std::optional @@ -134,10 +134,10 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) return std::nullopt; } if (points_with_twist.size() == 1) { - if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 < autoware_universe_utils::getLongitudinalVelocity(points_with_twist.front())) { return true; } - if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 > autoware_universe_utils::getLongitudinalVelocity(points_with_twist.front())) { return false; } return std::nullopt; @@ -181,8 +181,8 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) constexpr double eps = 1.0E-08; for (size_t i = start_idx + 1; i < points.size(); ++i) { - const auto prev_p = tier4_autoware_utils::getPoint(dst.back()); - const auto curr_p = tier4_autoware_utils::getPoint(points.at(i)); + const auto prev_p = autoware_universe_utils::getPoint(dst.back()); + const auto curr_p = autoware_universe_utils::getPoint(points.at(i)); if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } @@ -298,7 +298,7 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = autoware_universe_utils::calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { min_dist = dist; min_idx = i; @@ -350,13 +350,13 @@ std::optional findNearestIndex( size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto squared_dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose); + const auto squared_dist = autoware_universe_utils::calcSquaredDistance2d(points.at(i), pose); if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) { continue; } - const auto yaw = - tier4_autoware_utils::calcYawDeviation(tier4_autoware_utils::getPose(points.at(i)), pose); + const auto yaw = autoware_universe_utils::calcYawDeviation( + autoware_universe_utils::getPose(points.at(i)), pose); if (std::fabs(yaw) > max_yaw) { continue; } @@ -408,7 +408,7 @@ double calcLongitudinalOffsetToSegment( "[motion_utils] " + std::string(__func__) + ": Failed to calculate longitudinal offset because the given segment index is out of the " "points size."); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -436,7 +436,7 @@ double calcLongitudinalOffsetToSegment( const std::string error_message( "[motion_utils] " + std::string(__func__) + ": Longitudinal offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -447,8 +447,8 @@ double calcLongitudinalOffsetToSegment( return std::nan(""); } - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_front = autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; @@ -598,7 +598,7 @@ double calcLateralOffset( const std::string error_message( "[motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -613,8 +613,8 @@ double calcLateralOffset( const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices; const auto p_back_idx = p_front_idx + 1; - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx)); - const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx)); + const auto p_front = autoware_universe_utils::getPoint(overlap_removed_points.at(p_front_idx)); + const auto p_back = autoware_universe_utils::getPoint(overlap_removed_points.at(p_back_idx)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; @@ -671,7 +671,7 @@ double calcLateralOffset( const std::string error_message( "[motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -723,7 +723,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t double dist_sum = 0.0; for (size_t i = src_idx; i < dst_idx; ++i) { - dist_sum += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); } return dist_sum; } @@ -771,7 +771,7 @@ std::vector calcSignedArcLengthPartialSum( double dist_sum = 0.0; partial_dist.push_back(dist_sum); for (size_t i = src_idx; i < dst_idx - 1; ++i) { - dist_sum += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); partial_dist.push_back(dist_sum); } return partial_dist; @@ -958,10 +958,10 @@ std::vector calcCurvature(const T & points) } for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); - const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); - const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); - curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3)); + const auto p1 = autoware_universe_utils::getPoint(points.at(i - 1)); + const auto p2 = autoware_universe_utils::getPoint(points.at(i)); + const auto p3 = autoware_universe_utils::getPoint(points.at(i + 1)); + curvature_vec.at(i) = (autoware_universe_utils::calcCurvature(p1, p2, p3)); } curvature_vec.at(0) = curvature_vec.at(1); curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); @@ -996,12 +996,13 @@ std::vector> calcCurvatureAndArcLength(const T & point std::vector> curvature_arc_length_vec; curvature_arc_length_vec.emplace_back(0.0, 0.0); for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); - const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); - const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); - const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); - const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + const auto p1 = autoware_universe_utils::getPoint(points.at(i - 1)); + const auto p2 = autoware_universe_utils::getPoint(points.at(i)); + const auto p3 = autoware_universe_utils::getPoint(points.at(i + 1)); + const double curvature = autoware_universe_utils::calcCurvature(p1, p2, p3); + const double arc_length = + autoware_universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + + autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); curvature_arc_length_vec.emplace_back(curvature, arc_length); } curvature_arc_length_vec.emplace_back(0.0, 0.0); @@ -1076,7 +1077,7 @@ std::optional calcLongitudinalOffsetPoint( "[motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1092,7 +1093,7 @@ std::optional calcLongitudinalOffsetPoint( } if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPoint(points.at(src_idx)); + return autoware_universe_utils::getPoint(points.at(src_idx)); } if (offset < 0.0) { @@ -1108,12 +1109,12 @@ std::optional calcLongitudinalOffsetPoint( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware_universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPoint( + return autoware_universe_utils::calcInterpolatedPoint( p_back, p_front, std::abs(dist_res / dist_segment)); } } @@ -1207,7 +1208,7 @@ std::optional calcLongitudinalOffsetPose( "[motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1221,7 +1222,7 @@ std::optional calcLongitudinalOffsetPose( } if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPose(points.at(src_idx)); + return autoware_universe_utils::getPose(points.at(src_idx)); } if (offset < 0.0) { @@ -1234,12 +1235,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = reverse_points.at(i); const auto & p_back = reverse_points.at(i + 1); - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware_universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( + return autoware_universe_utils::calcInterpolatedPose( p_back, p_front, std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1251,12 +1252,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware_universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( + return autoware_universe_utils::calcInterpolatedPose( p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1356,8 +1357,8 @@ std::optional insertTargetPoint( return {}; } - const auto p_front = tier4_autoware_utils::getPoint(points.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(points.at(seg_idx + 1)); + const auto p_front = autoware_universe_utils::getPoint(points.at(seg_idx)); + const auto p_back = autoware_universe_utils::getPoint(points.at(seg_idx + 1)); try { validateNonSharpAngle(p_front, p_target, p_back); @@ -1367,9 +1368,9 @@ std::optional insertTargetPoint( } const auto overlap_with_front = - tier4_autoware_utils::calcDistance2d(p_target, p_front) < overlap_threshold; + autoware_universe_utils::calcDistance2d(p_target, p_front) < overlap_threshold; const auto overlap_with_back = - tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold; + autoware_universe_utils::calcDistance2d(p_target, p_back) < overlap_threshold; const auto is_driving_forward = isDrivingForward(points); if (!is_driving_forward) { @@ -1379,31 +1380,31 @@ std::optional insertTargetPoint( geometry_msgs::msg::Pose target_pose; { const auto p_base = is_driving_forward.value() ? p_back : p_front; - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base); + const auto pitch = autoware_universe_utils::calcElevationAngle(p_target, p_base); + const auto yaw = autoware_universe_utils::calcAzimuthAngle(p_target, p_base); target_pose.position = p_target; - target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + target_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } auto p_insert = points.at(seg_idx); - tier4_autoware_utils::setPose(target_pose, p_insert); + autoware_universe_utils::setPose(target_pose, p_insert); geometry_msgs::msg::Pose base_pose; { const auto p_base = is_driving_forward.value() ? p_front : p_back; - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target); + const auto pitch = autoware_universe_utils::calcElevationAngle(p_base, p_target); + const auto yaw = autoware_universe_utils::calcAzimuthAngle(p_base, p_target); - base_pose.position = tier4_autoware_utils::getPoint(p_base); - base_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + base_pose.position = autoware_universe_utils::getPoint(p_base); + base_pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } if (!overlap_with_front && !overlap_with_back) { if (is_driving_forward.value()) { - tier4_autoware_utils::setPose(base_pose, points.at(seg_idx)); + autoware_universe_utils::setPose(base_pose, points.at(seg_idx)); } else { - tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1)); + autoware_universe_utils::setPose(base_pose, points.at(seg_idx + 1)); } points.insert(points.begin() + seg_idx + 1, p_insert); return seg_idx + 1; @@ -1537,9 +1538,9 @@ std::optional insertTargetPoint( const double target_length = insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); - const auto p_target = tier4_autoware_utils::calcInterpolatedPoint( - tier4_autoware_utils::getPoint(points.at(*segment_idx)), - tier4_autoware_utils::getPoint(points.at(*segment_idx + 1)), ratio); + const auto p_target = autoware_universe_utils::calcInterpolatedPoint( + autoware_universe_utils::getPoint(points.at(*segment_idx)), + autoware_universe_utils::getPoint(points.at(*segment_idx + 1)), ratio); return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } @@ -1643,7 +1644,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware_universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1685,9 +1686,9 @@ std::optional insertStopPoint( double accumulated_length = 0; for (size_t i = 0; i < points_with_twist.size() - 1; ++i) { - const auto curr_pose = tier4_autoware_utils::getPose(points_with_twist.at(i)); - const auto next_pose = tier4_autoware_utils::getPose(points_with_twist.at(i + 1)); - const double length = tier4_autoware_utils::calcDistance2d(curr_pose, next_pose); + const auto curr_pose = autoware_universe_utils::getPose(points_with_twist.at(i)); + const auto next_pose = autoware_universe_utils::getPose(points_with_twist.at(i + 1)); + const double length = autoware_universe_utils::calcDistance2d(curr_pose, next_pose); if (accumulated_length + length + overlap_threshold > distance_to_stop_point) { const double insert_length = distance_to_stop_point - accumulated_length; return insertStopPoint(i, insert_length, points_with_twist, overlap_threshold); @@ -1747,7 +1748,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware_universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1794,7 +1795,7 @@ std::optional insertStopPoint( } for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware_universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return insert_idx; @@ -1834,8 +1835,8 @@ std::optional insertDecelPoint( for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { const auto & original_velocity = - tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(i)); - tier4_autoware_utils::setLongitudinalVelocity( + autoware_universe_utils::getLongitudinalVelocity(points_with_twist.at(i)); + autoware_universe_utils::setLongitudinalVelocity( std::min(original_velocity, velocity), points_with_twist.at(i)); } @@ -1858,30 +1859,30 @@ void insertOrientation(T & points, const bool is_driving_forward) { if (is_driving_forward) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & src_point = tier4_autoware_utils::getPoint(points.at(i)); - const auto & dst_point = tier4_autoware_utils::getPoint(points.at(i + 1)); - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware_universe_utils::getPoint(points.at(i)); + const auto & dst_point = autoware_universe_utils::getPoint(points.at(i + 1)); + const double pitch = autoware_universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware_universe_utils::calcAzimuthAngle(src_point, dst_point); + autoware_universe_utils::setOrientation( + autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); if (i == points.size() - 2) { // Terminal orientation is same as the point before it - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::getPose(points.at(i)).orientation, points.at(i + 1)); + autoware_universe_utils::setOrientation( + autoware_universe_utils::getPose(points.at(i)).orientation, points.at(i + 1)); } } } else { for (size_t i = points.size() - 1; i >= 1; --i) { - const auto & src_point = tier4_autoware_utils::getPoint(points.at(i)); - const auto & dst_point = tier4_autoware_utils::getPoint(points.at(i - 1)); - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware_universe_utils::getPoint(points.at(i)); + const auto & dst_point = autoware_universe_utils::getPoint(points.at(i - 1)); + const double pitch = autoware_universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware_universe_utils::calcAzimuthAngle(src_point, dst_point); + autoware_universe_utils::setOrientation( + autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); } // Initial orientation is same as the point after it - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::getPose(points.at(1)).orientation, points.at(0)); + autoware_universe_utils::setOrientation( + autoware_universe_utils::getPose(points.at(1)).orientation, points.at(0)); } } @@ -1906,14 +1907,14 @@ template void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { for (auto itr = points.begin(); std::next(itr) != points.end();) { - const auto p1 = tier4_autoware_utils::getPose(*itr); - const auto p2 = tier4_autoware_utils::getPose(*std::next(itr)); + const auto p1 = autoware_universe_utils::getPose(*itr); + const auto p2 = autoware_universe_utils::getPose(*std::next(itr)); const double yaw1 = tf2::getYaw(p1.orientation); const double yaw2 = tf2::getYaw(p2.orientation); if ( - max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) || - !tier4_autoware_utils::isDrivingForward(p1, p2)) { + max_yaw_diff < std::abs(autoware_universe_utils::normalizeRadian(yaw1 - yaw2)) || + !autoware_universe_utils::isDrivingForward(p1, p2)) { points.erase(std::next(itr)); return; } else { @@ -2066,9 +2067,9 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position); - const auto yaw = - tier4_autoware_utils::calcYawDeviation(tier4_autoware_utils::getPose(points.at(i)), pose); + autoware_universe_utils::calcSquaredDistance2d(points.at(i), pose.position); + const auto yaw = autoware_universe_utils::calcYawDeviation( + autoware_universe_utils::getPose(points.at(i)), pose); if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { if (is_within_constraints) { @@ -2099,7 +2100,7 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position); + autoware_universe_utils::calcSquaredDistance2d(points.at(i), pose.position); if (squared_dist_threshold < squared_dist) { if (is_within_constraints) { @@ -2278,7 +2279,7 @@ T cropForwardPoints( double sum_length = -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length) { const size_t end_idx = i; return T{points.begin(), points.begin() + end_idx}; @@ -2318,7 +2319,7 @@ T cropBackwardPoints( double sum_length = -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (int i = target_seg_idx; 0 < i; --i) { - sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < -backward_length) { const size_t begin_idx = i; return T{points.begin() + begin_idx, points.end()}; @@ -2417,7 +2418,7 @@ double calcYawDeviation( const std::string error_message( "[motion_utils] " + std::string(__func__) + " Given points size is less than 2. Failed to calculate yaw deviation."); - tier4_autoware_utils::print_backtrace(); + autoware_universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -2430,12 +2431,12 @@ double calcYawDeviation( const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); - const double path_yaw = tier4_autoware_utils::calcAzimuthAngle( - tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)), - tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); + const double path_yaw = autoware_universe_utils::calcAzimuthAngle( + autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx)), + autoware_universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); const double pose_yaw = tf2::getYaw(pose.orientation); - return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); + return autoware_universe_utils::normalizeRadian(pose_yaw - path_yaw); } extern template double calcYawDeviation>( diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index 21471a4a6a75e..c4ec4227f13e9 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -23,6 +23,7 @@ autoware_adapi_v1_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_msgs builtin_interfaces geometry_msgs @@ -31,7 +32,6 @@ rclcpp tf2 tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 6e9c45adc241b..9a01900517337 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,16 +14,16 @@ #include "motion_utils/marker/marker_helper.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" -#include +#include #include -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createDeletedDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createDeletedDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; using visualization_msgs::msg::MarkerArray; namespace @@ -92,7 +92,7 @@ visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + const auto pose_with_offset = autoware_universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, @@ -104,7 +104,7 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + const auto pose_with_offset = autoware_universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, @@ -116,7 +116,7 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + const auto pose_with_offset = autoware_universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 4a4e8dff1e4ad..1378c8921ee06 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,12 +14,12 @@ #include "motion_utils/resample/resample.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" #include "motion_utils/resample/resample_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" namespace motion_utils { @@ -50,7 +50,7 @@ std::vector resamplePointVector( for (size_t i = 1; i < points.size(); ++i) { const auto & prev_pt = points.at(i - 1); const auto & curr_pt = points.at(i); - const double ds = tier4_autoware_utils::calcDistance2d(prev_pt, curr_pt); + const double ds = autoware_universe_utils::calcDistance2d(prev_pt, curr_pt); input_arclength.push_back(ds + input_arclength.back()); x.push_back(curr_pt.x); y.push_back(curr_pt.y); @@ -144,7 +144,7 @@ std::vector resamplePoseVector( } const bool is_driving_forward = - tier4_autoware_utils::isDrivingForward(points.at(0), points.at(1)); + autoware_universe_utils::isDrivingForward(points.at(0), points.at(1)); motion_utils::insertOrientation(resampled_points, is_driving_forward); // Initial orientation is depend on the initial value of the resampled_arclength @@ -260,7 +260,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( const auto & prev_pt = input_path.points.at(i - 1).point; const auto & curr_pt = input_path.points.at(i).point; const double ds = - tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware_universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -449,7 +449,7 @@ autoware_planning_msgs::msg::Path resamplePath( const auto & prev_pt = input_path.points.at(i - 1); const auto & curr_pt = input_path.points.at(i); const double ds = - tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware_universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -603,7 +603,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const auto & prev_pt = input_trajectory.points.at(i - 1); const auto & curr_pt = input_trajectory.points.at(i); const double ds = - tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware_universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index 13df39daab59d..05e61b2120326 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -43,8 +43,8 @@ TrajectoryPoint calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = trajectory.points.at(segment_idx); const auto & next_pt = trajectory.points.at(segment_idx + 1); - const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_pose); + const auto v1 = autoware_universe_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = autoware_universe_utils::point2tfVector(curr_pt, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -57,7 +57,7 @@ TrajectoryPoint calcInterpolatedPoint( // pose interpolation interpolated_point.pose = - tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + autoware_universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -111,8 +111,8 @@ PathPointWithLaneId calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = path.points.at(segment_idx); const auto & next_pt = path.points.at(segment_idx + 1); - const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt.point, next_pt.point); - const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt.point, target_pose); + const auto v1 = autoware_universe_utils::point2tfVector(curr_pt.point, next_pt.point); + const auto v2 = autoware_universe_utils::point2tfVector(curr_pt.point, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -125,7 +125,7 @@ PathPointWithLaneId calcInterpolatedPoint( // pose interpolation interpolated_point.point.pose = - tier4_autoware_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio); + autoware_universe_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index 4c86135811003..90a7bff4f47ec 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -116,15 +116,15 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( // apply beta to CoG pose geometry_msgs::msg::Pose cog_pose_with_beta; - cog_pose_with_beta.position = tier4_autoware_utils::getPoint(path.points.at(i)); + cog_pose_with_beta.position = autoware_universe_utils::getPoint(path.points.at(i)); cog_pose_with_beta.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta); + autoware_universe_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta); const auto rear_pose = - tier4_autoware_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); + autoware_universe_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); // update pose - tier4_autoware_utils::setPose(rear_pose, cog_path.points.at(i)); + autoware_universe_utils::setPose(rear_pose, cog_path.points.at(i)); } // compensate for the last pose diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index ef374f0b484db..545a71aa0db08 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "motion_utils/constants.hpp" #include "motion_utils/resample/resample.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include @@ -31,9 +31,9 @@ using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromRPY; +using autoware_universe_utils::transformPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -251,7 +251,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -276,7 +276,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -547,7 +547,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, tier4_autoware_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, + i * 1.0, 0.0, 0.0, autoware_universe_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); } path.points.back().point.is_final = true; @@ -639,7 +639,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -659,7 +659,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } path.points.back().point.is_final = true; path.points.at(0).point.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -750,7 +750,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -758,7 +758,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -919,7 +919,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1045,7 +1045,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) } // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1070,7 +1070,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(0.0); + const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(0.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -1590,7 +1590,7 @@ TEST(resample_path, resample_path_by_vector) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1610,7 +1610,7 @@ TEST(resample_path, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -1884,7 +1884,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1902,7 +1902,7 @@ TEST(resample_path, resample_path_by_vector_backward) path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); } path.points.at(0).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -1969,7 +1969,7 @@ TEST(resample_path, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -1977,7 +1977,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -2103,7 +2103,7 @@ TEST(resample_path, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -2205,7 +2205,7 @@ TEST(resample_path, resample_path_by_same_interval) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2225,7 +2225,7 @@ TEST(resample_path, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -2660,7 +2660,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2681,7 +2681,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = traj.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -3013,7 +3013,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_traj.points.size(); ++i) { const auto p = resampled_traj.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -3122,7 +3122,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware_universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -3143,7 +3143,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = traj.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 7aae860b76533..4ba7ab9d5d859 100644 --- a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -23,8 +23,8 @@ namespace { using autoware_planning_msgs::msg::Trajectory; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromRPY; constexpr double epsilon = 1e-6; diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 90b11e535c486..7b1a57b229816 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include @@ -29,9 +29,9 @@ namespace { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromRPY; +using autoware_universe_utils::transformPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 6dd60c499d181..be35ae4c87135 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.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 "motion_utils/trajectory/path_with_lane_id.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -22,7 +22,7 @@ namespace { -using tier4_autoware_utils::createPoint; +using autoware_universe_utils::createPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -31,7 +31,7 @@ geometry_msgs::msg::Pose createPose( { geometry_msgs::msg::Pose p; p.position = createPoint(x, y, z); - p.orientation = tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + p.orientation = autoware_universe_utils::createQuaternionFromRPY(roll, pitch, yaw); return p; } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 3789ea21a2256..9c7281048f9f8 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include @@ -28,9 +28,9 @@ namespace { using autoware_planning_msgs::msg::Trajectory; using TrajectoryPointArray = std::vector; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromRPY; +using autoware_universe_utils::transformPoint; constexpr double epsilon = 1e-6; @@ -136,8 +136,8 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_universe_utils::pi; using motion_utils::validateNonSharpAngle; - using tier4_autoware_utils::pi; TrajectoryPoint p1; p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -1009,8 +1009,8 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) { + using autoware_universe_utils::deg2rad; using motion_utils::calcDistanceToForwardStopPoint; - using tier4_autoware_utils::deg2rad; const auto max_d = std::numeric_limits::max(); auto traj_input = generateTestTrajectory(100, 1.0, 3.0); @@ -1061,10 +1061,10 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) TEST(trajectory, calcLongitudinalOffsetPointFromIndex) { + using autoware_universe_utils::getPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1137,11 +1137,11 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) TEST(trajectory, calcLongitudinalOffsetPointFromPoint) { + using autoware_universe_utils::createPoint; + using autoware_universe_utils::getPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1215,10 +1215,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) { + using autoware_universe_utils::getPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1300,9 +1300,9 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_universe_utils::deg2rad; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1386,9 +1386,9 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_universe_utils::deg2rad; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1477,11 +1477,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) { + using autoware_universe_utils::createPoint; + using autoware_universe_utils::getPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1564,11 +1564,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) { using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1638,11 +1638,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1715,13 +1715,13 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) TEST(trajectory, insertTargetPoint) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1928,19 +1928,19 @@ TEST(trajectory, insertTargetPoint) TEST(trajectory, insertTargetPoint_Reverse) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::createQuaternionFromYaw; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; constexpr double overlap_threshold = 1e-4; auto traj = generateTestTrajectory(10, 1.0); for (size_t i = 0; i < traj.points.size(); ++i) { - traj.points.at(i).pose.orientation = createQuaternionFromYaw(tier4_autoware_utils::pi); + traj.points.at(i).pose.orientation = createQuaternionFromYaw(autoware_universe_utils::pi); } const auto total_length = calcArcLength(traj.points); @@ -1986,13 +1986,13 @@ TEST(trajectory, insertTargetPoint_Reverse) TEST(trajectory, insertTargetPoint_OverlapThreshold) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; constexpr double overlap_threshold = 1e-4; const auto traj = generateTestTrajectory(10, 1.0); @@ -2079,13 +2079,13 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) TEST(trajectory, insertTargetPoint_Length) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2317,13 +2317,13 @@ TEST(trajectory, insertTargetPoint_Length) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2503,13 +2503,13 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Idx) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); @@ -2733,13 +2733,13 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); @@ -2920,13 +2920,13 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero TEST(trajectory, insertTargetPoint_Length_from_a_pose) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -3272,13 +3272,13 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) TEST(trajectory, insertStopPoint_from_a_source_index) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3527,13 +3527,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) TEST(trajectory, insertStopPoint_from_front_point) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3724,13 +3724,13 @@ TEST(trajectory, insertStopPoint_from_front_point) TEST(trajectory, insertStopPoint_from_a_pose) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -4110,13 +4110,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) TEST(trajectory, insertStopPoint_with_pose_and_segment_index) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::deg2rad; + using autoware_universe_utils::getPose; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 3.0); const auto total_length = calcArcLength(traj.points); @@ -4354,12 +4354,12 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) TEST(trajectory, insertDecelPoint_from_a_point) { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::getLongitudinalVelocity; using motion_utils::calcArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertDecelPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getLongitudinalVelocity; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -4444,9 +4444,9 @@ TEST(trajectory, insertDecelPoint_from_a_point) TEST(trajectory, findFirstNearestIndexWithSoftConstraints) { + using autoware_universe_utils::pi; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; - using tier4_autoware_utils::pi; const auto traj = generateTestTrajectory(10, 1.0); @@ -5252,25 +5252,25 @@ TEST(trajectory, cropForwardPoints) { // Normal case const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); + cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(7)); } { // Forward length is longer than points arc length. const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); + cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); + cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); + cropForwardPoints(traj.points, autoware_universe_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } @@ -5283,25 +5283,25 @@ TEST(trajectory, cropBackwardPoints) { // Normal case const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); + cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(6)); } { // Backward length is longer than points arc length. const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); + cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); + cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); + cropBackwardPoints(traj.points, autoware_universe_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } @@ -5314,25 +5314,25 @@ TEST(trajectory, cropPoints) { // Normal case const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); + cropPoints(traj.points, autoware_universe_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); EXPECT_EQ(cropped_traj_points.size(), static_cast(3)); } { // Length is longer than points arc length. const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); + cropPoints(traj.points, autoware_universe_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); + cropPoints(traj.points, autoware_universe_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); + cropPoints(traj.points, autoware_universe_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } @@ -5357,7 +5357,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i)); const double yaw1 = tf2::getYaw(modified_traj.points.at(i).pose.orientation); const double yaw2 = tf2::getYaw(modified_traj.points.at(i + 1).pose.orientation); - const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)); + const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian(yaw1 - yaw2)); EXPECT_LE(yaw_diff, max_yaw_diff); } }; @@ -5420,8 +5420,8 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_universe_utils::createPoint; using motion_utils::isTargetPointFront; - using tier4_autoware_utils::createPoint; // Generate test trajectory const auto trajectory = generateTestTrajectory(10, 1.0); diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp index 540aef70cc1f4..826539458175c 100644 --- a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp +++ b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/vehicle/vehicle_state_checker.hpp" #include "test_vehicle_state_checker_helper.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -32,12 +32,12 @@ constexpr double STOP_DURATION_THRESHOLD_600_MS = 0.6; constexpr double STOP_DURATION_THRESHOLD_800_MS = 0.8; constexpr double STOP_DURATION_THRESHOLD_1000_MS = 1.0; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternion; +using autoware_universe_utils::createTranslation; using motion_utils::VehicleArrivalChecker; using motion_utils::VehicleStopChecker; using nav_msgs::msg::Odometry; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternion; -using tier4_autoware_utils::createTranslation; class CheckerNode : public rclcpp::Node { diff --git a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp index e9b924023f62e..eb8a7b81dbfb9 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp @@ -15,9 +15,9 @@ #ifndef OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ #define OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "object_recognition_utils/geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -28,7 +28,7 @@ namespace object_recognition_utils { -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Polygon2d; // minimum area to avoid division by zero static const double MIN_AREA = 1e-6; @@ -37,7 +37,7 @@ inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon boost::geometry::model::multi_polygon union_polygons; boost::geometry::union_(source_polygon, target_polygon, union_polygons); - tier4_autoware_utils::Polygon2d hull; + autoware_universe_utils::Polygon2d hull; boost::geometry::convex_hull(union_polygons, hull); return boost::geometry::area(hull); } @@ -67,9 +67,9 @@ inline double getUnionArea(const Polygon2d & source_polygon, const Polygon2d & t template double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -84,9 +84,9 @@ double get2dIoU(const T1 source_object, const T2 target_object, const double min template double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -100,10 +100,10 @@ double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) template double get2dPrecision(const T1 source_object, const T2 target_object) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); const double source_area = boost::geometry::area(source_polygon); if (source_area < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -115,9 +115,9 @@ double get2dPrecision(const T1 source_object, const T2 target_object) template double get2dRecall(const T1 source_object, const T2 target_object) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_universe_utils::toPolygon2d(target_object); const double target_area = boost::geometry::area(target_polygon); if (target_area < MIN_AREA) return 0.0; diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 1cd838e090560..72e70185d553b 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -15,7 +15,7 @@ #ifndef OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ #define OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index fdf73e6b26057..e7eaea96907f2 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -14,6 +14,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils geometry_msgs interpolation libboost-dev @@ -24,7 +25,6 @@ std_msgs tf2 tf2_eigen - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index fe9499b4eec33..e748d1165bef7 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -38,7 +38,7 @@ boost::optional calcInterpolatedPose( if (relative_time - epsilon < time_step * path_idx) { const double offset = relative_time - time_step * (path_idx - 1); const double ratio = std::clamp(offset / time_step, 0.0, 1.0); - return tier4_autoware_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); + return autoware_universe_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); } } @@ -90,7 +90,7 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( // Set Position for (size_t i = 0; i < resampled_size; ++i) { - const auto p = tier4_autoware_utils::createPoint( + const auto p = autoware_universe_utils::createPoint( interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); resampled_path.path.at(i).position = p; resampled_path.path.at(i).orientation = interpolated_quat.at(i); diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/object_recognition_utils/test/src/test_conversion.cpp index 378f9a1245746..a9b462f9ec4c4 100644 --- a/common/object_recognition_utils/test/src/test_conversion.cpp +++ b/common/object_recognition_utils/test/src/test_conversion.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 "object_recognition_utils/conversion.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/object_recognition_utils/test/src/test_matching.cpp index 2ad34c3ae6bbe..1481492e40e71 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/object_recognition_utils/test/src/test_matching.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/matching.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Point3d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Point3d; constexpr double epsilon = 1e-06; @@ -30,7 +30,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double { geometry_msgs::msg::Pose p; p.position = geometry_msgs::build().x(x).y(y).z(0.0); - p.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + p.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); return p; } } // namespace diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index 3dc3a8bcba3f7..94136c9c4da9c 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Point3d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Point3d; constexpr double epsilon = 1e-06; namespace { using autoware_perception_msgs::msg::PredictedPath; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromRPY; +using autoware_universe_utils::transformPoint; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -62,10 +62,10 @@ PredictedPath createTestPredictedPath( TEST(predicted_path_utils, testCalcInterpolatedPose) { + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::createQuaternionFromYaw; + using autoware_universe_utils::deg2rad; using object_recognition_utils::calcInterpolatedPose; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; const auto path = createTestPredictedPath(100, 0.1, 1.0); @@ -128,10 +128,10 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) TEST(predicted_path_utils, resamplePredictedPath_by_vector) { + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::createQuaternionFromYaw; + using autoware_universe_utils::deg2rad; using object_recognition_utils::resamplePredictedPath; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; const auto path = createTestPredictedPath(10, 1.0, 1.0); @@ -205,10 +205,10 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) { + using autoware_universe_utils::createQuaternionFromRPY; + using autoware_universe_utils::createQuaternionFromYaw; + using autoware_universe_utils::deg2rad; using object_recognition_utils::resamplePredictedPath; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; const auto path = createTestPredictedPath(10, 1.0, 1.0); diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index 0ccc6419b13de..c4641dc1e0db5 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -11,10 +11,10 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils motion_utils rclcpp rclcpp_components - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp index 838afb903c55f..ceef07ca268c4 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -15,8 +15,8 @@ #ifndef PATH_DISTANCE_CALCULATOR_HPP_ #define PATH_DISTANCE_CALCULATOR_HPP_ +#include #include -#include #include #include @@ -30,7 +30,7 @@ class PathDistanceCalculator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_path_; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; + autoware_universe_utils::SelfPoseListener self_pose_listener_; autoware_planning_msgs::msg::Path::SharedPtr path_; }; diff --git a/common/tier4_autoware_utils/README.md b/common/tier4_autoware_utils/README.md deleted file mode 100644 index ffcc414cd76ce..0000000000000 --- a/common/tier4_autoware_utils/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# tier4_autoware_utils - -## Purpose - -This package contains many common functions used by other packages, so please refer to them as needed. - -## For developers - -`tier4_autoware_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index ab6a0b25fd3b4..6d7109fbfa061 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -61,22 +61,22 @@ void visualizeBound( const auto [normal_vector_angle, adaptive_width] = [&]() -> std::pair { if (i == 0) { return std::make_pair( - tier4_autoware_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); + autoware_universe_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); } if (i == bound.size() - 1) { return std::make_pair( - tier4_autoware_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); + autoware_universe_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); } const auto & prev_p = bound.at(i - 1); const auto & curr_p = bound.at(i); const auto & next_p = bound.at(i + 1); - const float curr_to_prev_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, prev_p); - const float curr_to_next_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, next_p); + const float curr_to_prev_angle = autoware_universe_utils::calcAzimuthAngle(curr_p, prev_p); + const float curr_to_next_angle = autoware_universe_utils::calcAzimuthAngle(curr_p, next_p); const float normal_vector_angle = (curr_to_prev_angle + curr_to_next_angle) / 2.0; const float diff_angle = - tier4_autoware_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); + autoware_universe_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); if (diff_angle == 0.0) { return std::make_pair(normal_vector_angle, width); } 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 9cb710c239029..8edae446c46b0 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 @@ -86,8 +86,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) { for (auto && path_point : msg_ptr->points) { if ( - !rviz_common::validateFloats(tier4_autoware_utils::getPose(path_point)) && - !rviz_common::validateFloats(tier4_autoware_utils::getLongitudinalVelocity(path_point))) { + !rviz_common::validateFloats(autoware_universe_utils::getPose(path_point)) && + !rviz_common::validateFloats(autoware_universe_utils::getLongitudinalVelocity(path_point))) { return false; } } @@ -358,8 +358,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); - const auto & pose = tier4_autoware_utils::getPose(path_point); - const auto & velocity = tier4_autoware_utils::getLongitudinalVelocity(path_point); + const auto & pose = autoware_universe_utils::getPose(path_point); + const auto & velocity = autoware_universe_utils::getLongitudinalVelocity(path_point); // path if (property_path_view_.getBool()) { @@ -454,9 +454,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx; const auto & prev_path_pos = - tier4_autoware_utils::getPose(msg_ptr->points.at(prev_idx)).position; + autoware_universe_utils::getPose(msg_ptr->points.at(prev_idx)).position; const auto & next_path_pos = - tier4_autoware_utils::getPose(msg_ptr->points.at(next_idx)).position; + autoware_universe_utils::getPose(msg_ptr->points.at(next_idx)).position; Ogre::Vector3 position; position.x = pose.position.x; @@ -466,7 +466,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->setPosition(position); rviz_rendering::MovableText * text = slope_texts_.at(point_idx); - const double slope = tier4_autoware_utils::calcElevationAngle(prev_path_pos, next_path_pos); + const double slope = + autoware_universe_utils::calcElevationAngle(prev_path_pos, next_path_pos); std::stringstream ss; ss << std::fixed << std::setprecision(2) << slope; @@ -510,7 +511,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { const auto & point = msg_ptr->points.at(p_idx); - const auto & pose = tier4_autoware_utils::getPose(point); + const auto & pose = autoware_universe_utils::getPose(point); // footprint if (property_footprint_view_.getBool()) { Ogre::ColourValue color; diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp index 94943a25f6a64..c82f5b5df3885 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp @@ -15,8 +15,8 @@ #ifndef TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ #define TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ -#include -#include +#include +#include #include @@ -29,7 +29,7 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) // 1. check velocity const double target_velocity = - tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); + autoware_universe_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); if (epsilon < target_velocity) { return true; } else if (target_velocity < -epsilon) { @@ -46,13 +46,13 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) const size_t first_idx = is_last_point ? target_idx - 1 : target_idx; const size_t second_idx = is_last_point ? target_idx : target_idx + 1; - const auto first_pose = tier4_autoware_utils::getPose(points_with_twist.at(first_idx)); - const auto second_pose = tier4_autoware_utils::getPose(points_with_twist.at(second_idx)); + const auto first_pose = autoware_universe_utils::getPose(points_with_twist.at(first_idx)); + const auto second_pose = autoware_universe_utils::getPose(points_with_twist.at(second_idx)); const double first_traj_yaw = tf2::getYaw(first_pose.orientation); const double driving_direction_yaw = - tier4_autoware_utils::calcAzimuthAngle(first_pose.position, second_pose.position); + autoware_universe_utils::calcAzimuthAngle(first_pose.position, second_pose.position); if ( - std::abs(tier4_autoware_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < + std::abs(autoware_universe_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < M_PI_2) { return true; } diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 95edcb7a96c56..f1089e2421927 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_universe_utils autoware_vehicle_info_utils libqt5-core libqt5-gui @@ -24,7 +25,6 @@ rviz_rendering tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs ament_lint_auto diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml index bd72ecf97def4..bd888fb085960 100644 --- a/common/tier4_system_rviz_plugin/package.xml +++ b/common/tier4_system_rviz_plugin/package.xml @@ -11,6 +11,7 @@ autoware_cmake autoware_system_msgs + autoware_universe_utils diagnostic_msgs libqt5-core libqt5-gui @@ -19,7 +20,6 @@ rviz_common rviz_default_plugins rviz_ogre_vendor - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index 922ebd2c5e318..69057701886cb 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils autoware_vehicle_msgs libqt5-core libqt5-gui @@ -18,7 +19,6 @@ rviz_common rviz_default_plugins rviz_ogre_vendor - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp index 3b3810a782a91..a77e5769f7ff8 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp @@ -231,14 +231,14 @@ void AccelerationMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp index 8a962cea575e8..86adc54306262 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -21,11 +21,11 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" +#include #include #include #include #include -#include #include #endif @@ -67,8 +67,8 @@ private Q_SLOTS: private: static constexpr float meter_min_acceleration_ = -10.0f; static constexpr float meter_max_acceleration_ = 10.0f; - static constexpr float meter_min_angle_ = tier4_autoware_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = tier4_autoware_utils::deg2rad(320.f); + static constexpr float meter_min_angle_ = autoware_universe_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware_universe_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 5a9f7e602efc7..90bd6e91f9e4a 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -212,14 +212,14 @@ void ConsoleMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware_universe_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware_universe_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index 8f1a62fc35361..fc54208ddb8c8 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -21,12 +21,12 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" +#include +#include #include #include #include #include -#include -#include #include #endif @@ -63,10 +63,10 @@ private Q_SLOTS: // QImage hud_; private: - static constexpr float meter_min_velocity_ = tier4_autoware_utils::kmph2mps(0.f); - static constexpr float meter_max_velocity_ = tier4_autoware_utils::kmph2mps(60.f); - static constexpr float meter_min_angle_ = tier4_autoware_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = tier4_autoware_utils::deg2rad(320.f); + static constexpr float meter_min_velocity_ = autoware_universe_utils::kmph2mps(0.f); + static constexpr float meter_max_velocity_ = autoware_universe_utils::kmph2mps(60.f); + static constexpr float meter_min_angle_ = autoware_universe_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware_universe_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine diff --git a/common/traffic_light_recognition_marker_publisher/package.xml b/common/traffic_light_recognition_marker_publisher/package.xml index 2b65c5b868313..71a3e5238b388 100644 --- a/common/traffic_light_recognition_marker_publisher/package.xml +++ b/common/traffic_light_recognition_marker_publisher/package.xml @@ -14,11 +14,11 @@ autoware_map_msgs autoware_perception_msgs + autoware_universe_utils geometry_msgs lanelet2_extension rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ament_lint_auto 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 f11263e41da2b..b6dfdd88ded96 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 #include @@ -60,10 +60,10 @@ using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; @@ -235,14 +235,14 @@ class AEB : public rclcpp::Node explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ - this, "~/input/pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_velocity_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; + autoware_universe_utils::InterProcessPollingSubscriber sub_velocity_{ this, "~/input/velocity"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + autoware_universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 0404eec7ca308..e1bfd66376a0e 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_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater @@ -34,7 +35,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils 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 9201131f9ae0a..16a451702f5e5 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -14,12 +14,12 @@ #include "autoware/autonomous_emergency_braking/node.hpp" +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include #include #include @@ -70,29 +70,33 @@ Polygon2d createPolygon( appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + autoware_universe_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + autoware_universe_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + polygon, + autoware_universe_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + polygon, + autoware_universe_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + autoware_universe_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + autoware_universe_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + polygon, + autoware_universe_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + polygon, + autoware_universe_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); - polygon = tier4_autoware_utils::isClockwise(polygon) + polygon = autoware_universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware_universe_utils::inverseClockwise(polygon); Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); @@ -164,7 +168,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult AEB::onParameter( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); @@ -490,8 +494,8 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) double curr_y = 0.0; double curr_yaw = 0.0; geometry_msgs::msg::Pose ini_pose; - ini_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + ini_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, 0.0); + ini_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); if (std::abs(curr_v) < 0.1) { @@ -507,9 +511,9 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); - if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + current_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); + if (autoware_universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { continue; } path.push_back(current_pose); @@ -521,9 +525,9 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); - if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + current_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); + if (autoware_universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { continue; } path.push_back(current_pose); @@ -620,11 +624,11 @@ void AEB::createObjectDataUsingPointCloudClusters( const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); const auto & p = first_point_of_path.position; - return tier4_autoware_utils::createPoint(p.x, p.y, p.z); + return autoware_universe_utils::createPoint(p.x, p.y, p.z); }(); for (const auto & p : *points_belonging_to_cluster_hulls) { - const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z); + const auto obj_position = autoware_universe_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; @@ -688,36 +692,36 @@ void AEB::addMarker( const double color_r, const double color_g, const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers) { - auto path_marker = tier4_autoware_utils::createDefaultMarker( + auto path_marker = autoware_universe_utils::createDefaultMarker( "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.2), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_universe_utils::createMarkerScale(0.2, 0.2, 0.2), + autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); path_marker.points.resize(path.size()); for (size_t i = 0; i < path.size(); ++i) { path_marker.points.at(i) = path.at(i).position; } debug_markers.markers.push_back(path_marker); - auto polygon_marker = tier4_autoware_utils::createDefaultMarker( + auto polygon_marker = autoware_universe_utils::createDefaultMarker( "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, - tier4_autoware_utils::createMarkerScale(0.03, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_universe_utils::createMarkerScale(0.03, 0.0, 0.0), + autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto & poly : polygons) { for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { const auto & boost_cp = poly.outer().at(dp_idx); const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); - const auto curr_point = tier4_autoware_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); - const auto next_point = tier4_autoware_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + const auto curr_point = autoware_universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = autoware_universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); polygon_marker.points.push_back(curr_point); polygon_marker.points.push_back(next_point); } } debug_markers.markers.push_back(polygon_marker); - auto object_data_marker = tier4_autoware_utils::createDefaultMarker( + auto object_data_marker = autoware_universe_utils::createDefaultMarker( "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, - tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_universe_utils::createMarkerScale(0.5, 0.5, 0.5), + autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto & e : objects) { object_data_marker.points.push_back(e.position); } @@ -726,11 +730,11 @@ void AEB::addMarker( // Visualize planner type text if (closest_object.has_value()) { const auto & obj = closest_object.value(); - const auto color = tier4_autoware_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); - auto closest_object_velocity_marker_array = tier4_autoware_utils::createDefaultMarker( + const auto color = autoware_universe_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); + auto closest_object_velocity_marker_array = autoware_universe_utils::createDefaultMarker( "base_link", obj.stamp, ns + "_closest_object_velocity", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.7), color); + autoware_universe_utils::createMarkerScale(0.0, 0.0, 0.7), color); closest_object_velocity_marker_array.pose.position = obj.position; const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; closest_object_velocity_marker_array.text = @@ -744,10 +748,10 @@ void AEB::addMarker( void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) { - auto point_marker = tier4_autoware_utils::createDefaultMarker( + auto point_marker = autoware_universe_utils::createDefaultMarker( "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.3, 0.3, 0.3), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + autoware_universe_utils::createMarkerScale(0.3, 0.3, 0.3), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); } diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index d64ae915d4dc6..4434dc23ebf96 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -16,9 +16,9 @@ #define AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ #include "autoware/control_validator/debug_marker.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware_control_validator/msg/control_validator_status.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -69,9 +69,9 @@ class ControlValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); rclcpp::Subscription::SharedPtr sub_predicted_traj_; - tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_traj_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_reference_traj_{ this, "~/input/reference_trajectory"}; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml index 2059308ef9029..0b461f7322f82 100644 --- a/control/autoware_control_validator/package.xml +++ b/control/autoware_control_validator/package.xml @@ -21,6 +21,7 @@ rosidl_default_generators autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs @@ -28,7 +29,6 @@ nav_msgs rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp index 0020e0e8255ca..5a0d15b91a01e 100644 --- a/control/autoware_control_validator/src/debug_marker.cpp +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "autoware/control_validator/debug_marker.hpp" +#include #include -#include #include #include @@ -48,7 +48,7 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( void ControlValidatorDebugMarkerPublisher::pushPoseMarker( const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) { - using tier4_autoware_utils::createMarkerColor; + using autoware_universe_utils::createMarkerColor; // append arrow marker std_msgs::msg::ColorRGBA color; @@ -64,9 +64,9 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( { color = createMarkerColor(0.0, 0.0, 1.0, 0.999); } - Marker marker = tier4_autoware_utils::createDefaultMarker( + Marker marker = autoware_universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); + autoware_universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; @@ -76,10 +76,10 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( void ControlValidatorDebugMarkerPublisher::pushWarningMsg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware_universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware_universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -91,7 +91,7 @@ void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs:: const auto now = node_->get_clock()->now(); const auto stop_wall_marker = motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); - tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + autoware_universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } void ControlValidatorDebugMarkerPublisher::publish() diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp index a42834f998fcf..3dbcd3c4f92ae 100644 --- a/control/autoware_control_validator/src/utils.cpp +++ b/control/autoware_control_validator/src/utils.cpp @@ -14,9 +14,9 @@ #include "autoware/control_validator/utils.hpp" +#include #include #include -#include #include #include @@ -149,7 +149,7 @@ double calcMaxLateralDistance( alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); double max_dist = 0; for (const auto & point : alined_predicted_trajectory.points) { - const auto p0 = tier4_autoware_utils::getPoint(point); + const auto p0 = autoware_universe_utils::getPoint(point); // find nearest segment const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); diff --git a/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp index ec52fe89409bd..cea29ad887121 100644 --- a/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp @@ -17,8 +17,8 @@ #include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" +#include #include -#include #include #include @@ -70,9 +70,9 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_joy_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_joy_{ this, "input/joy"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "input/odometry"}; rclcpp::Time last_joy_received_time_; diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 10e33d5d6564a..3a9acf07a35bf 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -17,6 +17,7 @@ autoware_cmake autoware_control_msgs + autoware_universe_utils autoware_vehicle_msgs geometry_msgs joy @@ -26,7 +27,6 @@ sensor_msgs std_srvs tier4_api_utils - tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 2ec514b3f49e1..b5df59a45038c 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_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 #include @@ -50,9 +50,9 @@ namespace autoware::lane_departure_checker using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::PoseDeviation; -using tier4_autoware_utils::Segment2d; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::PoseDeviation; +using autoware_universe_utils::Segment2d; using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; @@ -122,7 +122,7 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool checkPathWillLeaveLane( diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index 1bc9ae8eee177..7f3fad0be4d28 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -16,12 +16,12 @@ #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ #include "autoware/lane_departure_checker/lane_departure_checker.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include +#include #include #include -#include -#include #include #include @@ -67,15 +67,15 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_route_{ this, "~/input/route"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ this, "~/input/reference_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ this, "~/input/predicted_trajectory"}; // Data Buffer @@ -99,8 +99,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); // Publisher - tier4_autoware_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - tier4_autoware_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware_universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; + autoware_universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp index 9c8f955f555d7..4435f282054ad 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/control/autoware_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/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index 7a08cd2907120..8901a1b4d5421 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -15,6 +15,7 @@ autoware_map_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils boost diagnostic_updater @@ -29,7 +30,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 570caedf2ea54..e1ef69e97fe0c 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -16,11 +16,11 @@ #include "autoware/lane_departure_checker/util/create_vehicle_footprint.hpp" +#include +#include +#include +#include #include -#include -#include -#include -#include #include @@ -30,12 +30,12 @@ #include #include +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::MultiPoint2d; +using autoware_universe_utils::MultiPolygon2d; +using autoware_universe_utils::Point2d; using motion_utils::calcArcLength; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::MultiPolygon2d; -using tier4_autoware_utils::Point2d; namespace { @@ -102,7 +102,7 @@ Output LaneDepartureChecker::update(const Input & input) { Output output{}; - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; output.trajectory_deviation = calcTrajectoryDeviation( *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, @@ -171,7 +171,7 @@ PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( { const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, pose, dist_threshold, yaw_threshold); - return tier4_autoware_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); + return autoware_universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); } TrajectoryPoints LaneDepartureChecker::resampleTrajectory( @@ -183,8 +183,8 @@ TrajectoryPoints LaneDepartureChecker::resampleTrajectory( for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(resampled.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware_universe_utils::fromMsg(resampled.back().pose.position); + const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); if (boost::geometry::distance(p1.to_2d(), p2.to_2d()) > interval) { resampled.push_back(point); @@ -205,8 +205,8 @@ TrajectoryPoints LaneDepartureChecker::cutTrajectory( for (size_t i = 1; i < trajectory.size(); ++i) { const auto & point = trajectory.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(cut.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware_universe_utils::fromMsg(cut.back().pose.position); + const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -251,7 +251,7 @@ std::vector LaneDepartureChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : trajectory) { vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.pose))); + transformVector(local_vehicle_footprint, autoware_universe_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -267,8 +267,8 @@ std::vector LaneDepartureChecker::createVehicleFootprints( // Create vehicle footprint on each Path point std::vector vehicle_footprints; for (const auto & p : path.points) { - vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.point.pose))); + vehicle_footprints.push_back(transformVector( + local_vehicle_footprint, autoware_universe_utils::pose2transform(p.point.pose))); } return vehicle_footprints; @@ -322,16 +322,18 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( +std::optional +LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); - auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { - tier4_autoware_utils::Polygon2d p; + auto to_polygon2d = + [](const lanelet::BasicPolygon2d & poly) -> autoware_universe_utils::Polygon2d { + autoware_universe_utils::Polygon2d p; auto & outer = p.outer(); for (const auto & p : poly) { - tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + autoware_universe_utils::Point2d p2d(p.x(), p.y()); outer.push_back(p2d); } boost::geometry::correct(p); @@ -339,15 +341,15 @@ std::optional LaneDepartureChecker::getFusedLan }; // Fuse lanelets into a single BasicPolygon2d - auto fused_lanelets = [&]() -> std::optional { + auto fused_lanelets = [&]() -> std::optional { if (lanelets_distance_pair.empty()) return std::nullopt; - tier4_autoware_utils::MultiPolygon2d lanelet_unions; - tier4_autoware_utils::MultiPolygon2d result; + autoware_universe_utils::MultiPolygon2d lanelet_unions; + autoware_universe_utils::MultiPolygon2d result; for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; const auto & p = route_lanelet.polygon2d().basicPolygon(); - tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + autoware_universe_utils::Polygon2d poly = to_polygon2d(p); boost::geometry::union_(lanelet_unions, poly, result); lanelet_unions = result; result.clear(); diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 41f07499a1996..84ab3877fe9f9 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -14,14 +14,14 @@ #include "autoware/lane_departure_checker/lane_departure_checker_node.hpp" +#include +#include +#include #include #include #include #include #include -#include -#include -#include #include @@ -31,7 +31,7 @@ #include #include -using tier4_autoware_utils::rad2deg; +using autoware_universe_utils::rad2deg; namespace { @@ -251,7 +251,7 @@ bool LaneDepartureCheckerNode::isDataValid() void LaneDepartureCheckerNode::onTimer() { std::map processing_time_map; - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; stop_watch.tic("Total"); current_odom_ = sub_odom_.takeData(); @@ -453,9 +453,9 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation( visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const { - 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; visualization_msgs::msg::MarkerArray marker_array; diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp index aa257f327b8c9..2edbfdd3e1e90 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #define AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "geometry_msgs/msg/point.hpp" @@ -117,7 +117,7 @@ class MPCTrajectory point.pose.position.x = x.at(i); point.pose.position.y = y.at(i); point.pose.position.z = z.at(i); - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw.at(i)); + point.pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw.at(i)); point.longitudinal_velocity_mps = vx.at(i); points.push_back(point); } diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 1e250dd89ce3f..0f0778010e24e 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -20,6 +20,7 @@ autoware_control_msgs autoware_planning_msgs autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs @@ -34,7 +35,6 @@ std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index f30e80dbba828..8db93efc127e2 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -15,19 +15,19 @@ #include "autoware/mpc_lateral_controller/mpc.hpp" #include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include namespace autoware::motion::control::mpc_lateral_controller { -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::rad2deg; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::normalizeRadian; +using autoware_universe_utils::rad2deg; MPC::MPC(rclcpp::Node & node) { @@ -499,7 +499,7 @@ MPCMatrix MPC::generateMPCMatrix( // get reference input (feed-forward) m_vehicle_model_ptr->setCurvature(ref_smooth_k); m_vehicle_model_ptr->calculateReferenceInput(Uref); - if (std::fabs(Uref(0, 0)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) { + if (std::fabs(Uref(0, 0)) < autoware_universe_utils::deg2rad(m_param.zero_ff_steer_deg)) { Uref(0, 0) = 0.0; // ignore curvature noise } m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; 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 25637c62d4809..e6865db227d44 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -616,7 +616,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const // TODO(Horibe): update implementation to check trajectory shape around ego vehicle. // Now temporally check the goal position. for (const auto & trajectory : m_trajectory_buffer) { - const auto change_distance = tier4_autoware_utils::calcDistance2d( + const auto change_distance = autoware_universe_utils::calcDistance2d( trajectory.points.back().pose, m_current_trajectory.points.back().pose); if (change_distance > m_new_traj_end_dist) { return true; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 83c0ba13fb350..9d0102432023a 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -14,11 +14,11 @@ #include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -42,9 +42,9 @@ double calcLongitudinalOffset( namespace MPCUtils { -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::normalizeRadian; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::createQuaternionFromYaw; +using autoware_universe_utils::normalizeRadian; double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2) { @@ -240,7 +240,7 @@ std::vector calcTrajectoryCurvature( p2.y = traj.y.at(curr_idx); p3.y = traj.y.at(next_idx); try { - curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3); + curvature_vec.at(curr_idx) = autoware_universe_utils::calcCurvature(p1, p2, p3); } catch (...) { std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl; curvature_vec.at(curr_idx) = 0.0; @@ -281,7 +281,7 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input) p.pose.position.x = input.x.at(i); p.pose.position.y = input.y.at(i); p.pose.position.z = input.z.at(i); - p.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(input.yaw.at(i)); + p.pose.orientation = autoware_universe_utils::createQuaternionFromYaw(input.yaw.at(i)); p.longitudinal_velocity_mps = static_cast(input.vx.at(i)); output.points.push_back(p); @@ -389,7 +389,7 @@ bool calcNearestPoseInterp( prev_traj_point.x = traj.x.at(prev); prev_traj_point.y = traj.y.at(prev); const double traj_seg_length = - tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point); + autoware_universe_utils::calcDistance2d(prev_traj_point, next_traj_point); /* if distance between two points are too close */ if (traj_seg_length < 1.0E-5) { nearest_pose->position.x = traj.x.at(*nearest_index); @@ -460,7 +460,7 @@ void extendTrajectoryInYawDirection( const double dt = interval / extend_vel; const size_t num_extended_point = static_cast(extend_dist / interval); for (size_t i = 0; i < num_extended_point; ++i) { - extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); + extended_pose = autoware_universe_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); traj.push_back( extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(), extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt); diff --git a/control/autoware_operation_mode_transition_manager/package.xml b/control/autoware_operation_mode_transition_manager/package.xml index c93b38c537a73..9e188136484cd 100644 --- a/control/autoware_operation_mode_transition_manager/package.xml +++ b/control/autoware_operation_mode_transition_manager/package.xml @@ -13,6 +13,7 @@ rosidl_default_generators autoware_control_msgs + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs @@ -22,7 +23,6 @@ rclcpp rclcpp_components std_srvs - tier4_autoware_utils tier4_control_msgs tier4_system_msgs tier4_vehicle_msgs diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp index 0cacb083fc293..46cc360355e67 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.hpp +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -18,10 +18,10 @@ #include "compatibility.hpp" #include "state.hpp" +#include #include #include #include -#include #include #include @@ -49,10 +49,10 @@ class OperationModeTransitionManager : public rclcpp::Node const ChangeOperationModeAPI::Service::Response::SharedPtr response); using ControlModeCommandType = ControlModeCommand::Request::_mode_type; - tier4_autoware_utils::InterProcessPollingSubscriber sub_control_mode_report_{ - this, "control_mode_report"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_gate_operation_mode_{ - this, "gate_operation_mode"}; + autoware_universe_utils::InterProcessPollingSubscriber + sub_control_mode_report_{this, "control_mode_report"}; + autoware_universe_utils::InterProcessPollingSubscriber + sub_gate_operation_mode_{this, "gate_operation_mode"}; rclcpp::Client::SharedPtr cli_control_mode_; rclcpp::Publisher::SharedPtr pub_debug_info_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp index 4eaf0ddf8e764..5433611e1df13 100644 --- a/control/autoware_operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -16,9 +16,9 @@ #include "util.hpp" +#include +#include #include -#include -#include #include #include @@ -26,9 +26,9 @@ namespace autoware::operation_mode_transition_manager { +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcYawDeviation; using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcYawDeviation; AutonomousMode::AutonomousMode(rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()) 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 a12beb6637b04..ed9fcb3974c83 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 @@ -21,13 +21,13 @@ #include "autoware/pid_longitudinal_controller/pid.hpp" #include "autoware/pid_longitudinal_controller/smooth_stop.hpp" #include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tier4_autoware_utils/ros/marker_helper.hpp" #include #include @@ -51,9 +51,9 @@ namespace autoware::motion::control::pid_longitudinal_controller { using autoware_adapi_v1_msgs::msg::OperationModeState; -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; using visualization_msgs::msg::Marker; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index a84083eae2725..fe5678aee93cd 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -22,6 +22,7 @@ autoware_control_msgs autoware_planning_msgs autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs @@ -35,7 +36,6 @@ std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 8fb6528dc431e..8ebf483d108b1 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -93,7 +93,7 @@ double getPitchByTraj( const auto [prev_idx, next_idx] = [&]() { for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance3d( + const double dist = autoware_universe_utils::calcDistance3d( trajectory.points.at(start_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) @@ -105,7 +105,7 @@ double getPitchByTraj( std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); }(); - return tier4_autoware_utils::calcElevationAngle( + return autoware_universe_utils::calcElevationAngle( trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position); } @@ -166,7 +166,7 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( double remain_dist = distance; geometry_msgs::msg::Pose p = trajectory.points.back().pose; for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) { - const double dist = tier4_autoware_utils::calcDistance3d( + const double dist = autoware_universe_utils::calcDistance3d( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose); if (remain_dist < dist) { if (remain_dist <= 0.0) { 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 0ff93b1564fec..6effe8d13dbdc 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -14,9 +14,9 @@ #include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -465,10 +465,10 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // check if the deviation is worth emergency m_diagnostic_data.trans_deviation = - tier4_autoware_utils::calcDistance2d(current_interpolated_pose.first, current_pose); + autoware_universe_utils::calcDistance2d(current_interpolated_pose.first, current_pose); const bool is_dist_deviation_large = m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; - m_diagnostic_data.rot_deviation = std::abs(tier4_autoware_utils::normalizeRadian( + m_diagnostic_data.rot_deviation = std::abs(autoware_universe_utils::normalizeRadian( tf2::getYaw(current_interpolated_pose.first.pose.orientation) - tf2::getYaw(current_pose.orientation))); const bool is_yaw_deviation_large = @@ -605,7 +605,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d auto marker = createDefaultMarker( "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = tier4_autoware_utils::calcOffsetPose( + marker.pose = autoware_universe_utils::calcOffsetPose( m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, m_vehicle_width / 2 + 2.0, 1.5); marker.text = "steering not\nconverged"; diff --git a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp index 72cab0a10d890..1030b58e0f979 100644 --- a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp @@ -33,9 +33,9 @@ #include "autoware/pure_pursuit/autoware_pure_pursuit.hpp" #include "autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include +#include #include -#include -#include #include #include @@ -78,10 +78,10 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::SelfPoseListener self_pose_listener_{this}; + autoware_universe_utils::InterProcessPollingSubscriber sub_trajectory_{this, "input/reference_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_current_odometry_{this, "input/current_odometry"}; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml index 138eda06926b5..64468eb7ce1b5 100644 --- a/control/autoware_pure_pursuit/package.xml +++ b/control/autoware_pure_pursuit/package.xml @@ -17,6 +17,7 @@ autoware_control_msgs autoware_planning_msgs autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils boost geometry_msgs @@ -31,7 +32,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs visualization_msgs diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp index eee22621e68ef..71fc315c92df4 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -141,7 +141,7 @@ TrajectoryPoint PurePursuitLateralController::calcNextPose( const double ds, TrajectoryPoint & point, Lateral cmd) const { geometry_msgs::msg::Transform transform; - transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); + transform.translation = autoware_universe_utils::createTranslation(ds, 0.0, 0.0); transform.rotation = planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); TrajectoryPoint output_p; @@ -202,10 +202,10 @@ double PurePursuitLateralController::calcCurvature(const size_t closest_idx) double current_curvature = 0.0; try { - current_curvature = tier4_autoware_utils::calcCurvature( - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(next_idx))); + current_curvature = autoware_universe_utils::calcCurvature( + autoware_universe_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), + autoware_universe_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), + autoware_universe_utils::getPoint(trajectory_resampled_->points.at(next_idx))); } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what()); diff --git a/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp index c9c23146baf1a..cffdbf2cdf2ab 100644 --- a/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp +++ b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ #define AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include @@ -43,11 +43,11 @@ class ShiftDecider : public rclcpp::Node void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_control_cmd_{this, "input/control_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_autoware_state_{this, "input/state"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_current_gear_{this, "input/current_gear"}; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_shift_decider/package.xml b/control/autoware_shift_decider/package.xml index 40708c2c73cf1..ebe86ddaac6d6 100644 --- a/control/autoware_shift_decider/package.xml +++ b/control/autoware_shift_decider/package.xml @@ -15,10 +15,10 @@ autoware_control_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs rclcpp rclcpp_components - tier4_autoware_utils ament_cmake_cppcheck ament_cmake_cpplint diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml index a05f06cd865ff..12601504dd23a 100644 --- a/control/autoware_smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -23,13 +23,13 @@ autoware_control_msgs autoware_planning_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs motion_utils pybind11_vendor python3-scipy rclcpp rclcpp_components - tier4_autoware_utils ros2launch diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index 283ab791c1ef9..5925fbebb1e7e 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -22,6 +22,7 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs @@ -36,7 +37,6 @@ std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp index 6be9e5e8a6480..06873c31cc6cc 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -18,18 +18,18 @@ #include "autoware/trajectory_follower_base/lateral_controller_base.hpp" #include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" #include "autoware/trajectory_follower_node/visibility_control.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include -#include +#include #include "autoware_control_msgs/msg/control.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" @@ -55,7 +55,7 @@ namespace trajectory_follower_node { using autoware_adapi_v1_msgs::msg::OperationModeState; -using tier4_autoware_utils::StopWatch; +using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -77,20 +77,20 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::shared_ptr lateral_controller_; // Subscribers - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_ref_path_{this, "~/input/reference_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/current_odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_steering_{this, "~/input/current_steering"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_accel_{this, "~/input/current_accel"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/current_operation_mode"}; // Publishers @@ -129,9 +129,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp index bce0c88e3aa28..0c179ad1996f4 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ #define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include @@ -44,9 +44,9 @@ class SimpleTrajectoryFollower : public rclcpp::Node ~SimpleTrajectoryFollower() = default; private: - tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_trajectory_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_trajectory_{ this, "~/input/trajectory"}; rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_trajectory_follower_node/package.xml b/control/autoware_trajectory_follower_node/package.xml index 0726b919d387e..3aeb3a814421f 100644 --- a/control/autoware_trajectory_follower_node/package.xml +++ b/control/autoware_trajectory_follower_node/package.xml @@ -26,12 +26,12 @@ autoware_planning_msgs autoware_pure_pursuit autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs motion_utils rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ros2launch diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index ff7827ea252a1..7876a38247bc9 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -17,7 +17,7 @@ #include "autoware/mpc_lateral_controller/mpc_lateral_controller.hpp" #include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include #include @@ -80,9 +80,10 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( @@ -222,10 +223,10 @@ void Controller::publishDebugMarker( // steer converged marker { - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); + autoware_universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware_universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); marker.pose = input_data.current_odometry.pose.pose; std::stringstream ss; diff --git a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp index 0b638ccda1bc2..0bd917b9abf7f 100644 --- a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -14,17 +14,17 @@ #include "autoware/trajectory_follower_node/simple_trajectory_follower.hpp" +#include #include -#include #include namespace simple_trajectory_follower { +using autoware_universe_utils::calcLateralDeviation; +using autoware_universe_utils::calcYawDeviation; using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcYawDeviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml index f67d01a5593ae..6c10d98505947 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_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs @@ -28,7 +29,6 @@ rclcpp_components std_srvs tier4_api_utils - tier4_autoware_utils tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs 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 79b43ebe299ba..7295032ae3cb7 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -14,8 +14,8 @@ #include "vehicle_cmd_gate.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include #include @@ -210,9 +210,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) timer_pub_status_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); // Parameter Callback set_param_res_ = @@ -222,7 +223,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; // Parameter updateParam(parameters, "use_emergency_handling", use_emergency_handling_); updateParam( 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 f69ac24d2a6f8..19e91d863e8f8 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -16,17 +16,17 @@ #define VEHICLE_CMD_GATE_HPP_ #include "adapi_pause_interface.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "moderate_stop_interface.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" +#include +#include #include #include #include #include #include -#include -#include #include #include @@ -155,31 +155,31 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ this, "input/auto/gear_cmd"}; void onAutoCtrlCmd(Control::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ this, "input/external/gear_cmd"}; void onRemoteCtrlCmd(Control::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ this, "input/emergency/gear_cmd"}; void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); @@ -261,9 +261,9 @@ class VehicleCmdGate : public rclcpp::Node MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::vehicle_cmd_gate diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 73d48db6400a5..b359eb16387db 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -19,9 +19,9 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" +#include #include #include -#include #include #include diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index b18fb8c98ccdc..64c775d66e794 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_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs @@ -40,7 +41,6 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils tier4_debug_msgs builtin_interfaces global_parameter_loader diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 51c818205e047..a4cca2e2a64d0 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -14,8 +14,8 @@ #include "control_performance_analysis/control_performance_analysis_core.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/interpolation.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -158,7 +158,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() // Compute the yaw angle error. const double heading_yaw_error = - tier4_autoware_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); + autoware_universe_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); // Set the values of ErrorMsgVars. @@ -174,7 +174,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() const double Vx = odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; // Current acceleration calculation - const auto ds = tier4_autoware_utils::calcDistance2d( + const auto ds = autoware_universe_utils::calcDistance2d( odom_history_ptr_->at(odom_size - 1).pose.pose, odom_history_ptr_->at(odom_size - 2).pose.pose); const double vel_mean = (odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x + @@ -371,7 +371,8 @@ std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() } auto fun_distance_cond = [this](auto point_t) { - const double dist = tier4_autoware_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); + const double dist = + autoware_universe_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); return dist > p_.wheelbase_; }; @@ -442,7 +443,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // Compute arc-length ds between 2 points. const double ds_arc_length = - tier4_autoware_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); + autoware_universe_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); // Define waypoints 10 meters behind the rear axle if exist. // If not exist, we will take the first point of the @@ -461,7 +462,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // We compute a curvature estimate from these points. double estimated_curvature = 0.0; try { - estimated_curvature = tier4_autoware_utils::calcCurvature( + estimated_curvature = autoware_universe_utils::calcCurvature( points.at(loc_of_back_idx).pose.position, points.at(idx_curve_ref_wp).pose.position, points.at(loc_of_forward_idx).pose.position); } catch (...) { @@ -486,7 +487,7 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() const double look_ahead_distance_pp = std::max(p_.wheelbase_, 2 * Vx); auto fun_distance_cond = [this, &look_ahead_distance_pp](auto point_t) { - const double dist = tier4_autoware_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); + const double dist = autoware_universe_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); return dist > look_ahead_distance_pp; }; 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 efaefd765e793..27f4405e26f6c 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 @@ -35,7 +35,7 @@ namespace obstacle_collision_checker { -using tier4_autoware_utils::LinearRing2d; +using autoware_universe_utils::LinearRing2d; struct Param { diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 9d79a0facac95..85b3e3d25c2eb 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -17,13 +17,13 @@ #include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include #include #include @@ -48,8 +48,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node private: // Subscriber - std::shared_ptr self_pose_listener_; - std::shared_ptr transform_listener_; + std::shared_ptr self_pose_listener_; + std::shared_ptr transform_listener_; rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; @@ -72,8 +72,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher - std::shared_ptr debug_publisher_; - std::shared_ptr time_publisher_; + std::shared_ptr debug_publisher_; + std::shared_ptr time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 795302abe75f4..45b142fe601fe 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_universe_utils autoware_vehicle_info_utils boost diagnostic_updater @@ -33,7 +34,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils 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 a844fe6a50cbf..e6c3aff83ee10 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 @@ -14,12 +14,12 @@ #include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include +#include +#include +#include #include #include -#include -#include -#include -#include #include @@ -90,7 +90,7 @@ ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) Output ObstacleCollisionChecker::update(const Input & input) { Output output; - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; // resample trajectory by braking distance constexpr double min_velocity = 0.01; @@ -131,8 +131,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(resampled.points.back().pose.position).to_2d(); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position).to_2d(); + const auto p1 = autoware_universe_utils::fromMsg(resampled.points.back().pose.position).to_2d(); + const auto p2 = autoware_universe_utils::fromMsg(point.pose.position).to_2d(); if (boost::geometry::distance(p1, p2) > interval) { resampled.points.push_back(point); @@ -154,8 +154,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware_universe_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -197,8 +197,8 @@ std::vector ObstacleCollisionChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : trajectory.points) { vehicle_footprints.push_back( - tier4_autoware_utils::transformVector( - local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.pose))); + autoware_universe_utils::transformVector( + local_vehicle_footprint, autoware_universe_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -221,7 +221,7 @@ std::vector ObstacleCollisionChecker::createVehiclePassingAreas( LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( const LinearRing2d & area1, const LinearRing2d & area2) { - tier4_autoware_utils::MultiPoint2d combined; + autoware_universe_utils::MultiPoint2d combined; for (const auto & p : area1) { combined.push_back(p); } @@ -256,7 +256,7 @@ bool ObstacleCollisionChecker::hasCollision( { for (const auto & point : obstacle_pointcloud.points) { if (boost::geometry::within( - tier4_autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { + autoware_universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( rclcpp::get_logger("obstacle_collision_checker"), "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); 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 402123c81d361..9bd95a985e2de 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 #include #include @@ -70,8 +70,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt obstacle_collision_checker_->setParam(param_); // Subscriber - self_pose_listener_ = std::make_shared(this); - transform_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), @@ -86,8 +86,9 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::onOdom, this, _1)); // Publisher - debug_publisher_ = std::make_shared(this, "debug/marker"); - time_publisher_ = std::make_shared(this); + debug_publisher_ = + std::make_shared(this, "debug/marker"); + time_publisher_ = std::make_shared(this); // Diagnostic Updater updater_.setHardwareID("obstacle_collision_checker"); @@ -282,9 +283,9 @@ void ObstacleCollisionCheckerNode::checkLaneDeparture( visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const { - 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; visualization_msgs::msg::MarkerArray marker_array; 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 2a7a552c1973d..bcc5f7dd4542a 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,15 +15,15 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include #include #include @@ -47,10 +47,10 @@ using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using PointArray = std::vector; namespace bg = boost::geometry; diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp index 02ade624d630c..84ccbad32202c 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp @@ -31,9 +31,9 @@ #define EIGEN_MPL2_ONLY +#include #include #include -#include namespace autoware::motion::control::predicted_path_checker { @@ -52,12 +52,12 @@ class PredictedPathCheckerDebugNode ~PredictedPathCheckerDebugNode() {} bool pushPolygon( - const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); 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 6c4832a0dac3e..48ff15b89389d 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 @@ -51,10 +51,10 @@ using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; struct NodeParam { @@ -88,7 +88,7 @@ class PredictedPathCheckerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_cli_; // Subscriber - std::shared_ptr self_pose_listener_; + std::shared_ptr self_pose_listener_; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; 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 d9299f09dfb6b..a6ad488950c09 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 @@ -43,12 +43,12 @@ using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using std_msgs::msg::Header; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using PointArray = std::vector; using TrajectoryPoints = std::vector; diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 911344c578414..6c888e1e7d189 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_universe_utils autoware_vehicle_info_utils boost component_interface_specs @@ -31,7 +32,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs 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 7d392571546b4..fb2eda3d04c33 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 @@ -14,8 +14,8 @@ #include "predicted_path_checker/collision_checker.hpp" +#include #include -#include #include #include @@ -80,12 +80,12 @@ CollisionChecker::checkTrajectoryForCollision( double distance_to_current = std::numeric_limits::max(); double distance_to_history = std::numeric_limits::max(); if (found_collision_at_dynamic_objects) { - distance_to_current = tier4_autoware_utils::calcDistance2d( + distance_to_current = autoware_universe_utils::calcDistance2d( p_front, found_collision_at_dynamic_objects.get().first); } if (found_collision_at_history) { distance_to_history = - tier4_autoware_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + autoware_universe_utils::calcDistance2d(p_front, found_collision_at_history.get().first); } else { predicted_object_history_.clear(); } @@ -140,7 +140,7 @@ CollisionChecker::checkObstacleHistory( bool is_init = false; std::pair nearest_collision_object_with_point; for (const auto & p : collision_points_in_history) { - double norm = tier4_autoware_utils::calcDistance2d(p.first, base_pose); + double norm = autoware_universe_utils::calcDistance2d(p.first, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; nearest_collision_object_with_point = p; diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp index 3fb7b69d531c0..e5a17ace712ae 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -14,29 +14,29 @@ #include "predicted_path_checker/debug_marker.hpp" +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif -#include +#include #include #include +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using motion_utils::createDeletedStopVirtualWallMarker; using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; namespace autoware::motion::control::predicted_path_checker { @@ -51,7 +51,7 @@ PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( } bool PredictedPathCheckerDebugNode::pushPolygon( - const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; for (const auto & point : polygon.outer()) { @@ -82,7 +82,7 @@ bool PredictedPathCheckerDebugNode::pushPolygon( } bool PredictedPathCheckerDebugNode::pushPolyhedron( - const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; 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 38717a1849ad9..505911dbc5b72 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 @@ -14,11 +14,11 @@ #include "predicted_path_checker/predicted_path_checker_node.hpp" +#include +#include #include #include #include -#include -#include #include #include @@ -67,7 +67,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n declare_parameter("collision_checker_params.chattering_threshold", 0.2); // Subscriber - self_pose_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); sub_dynamic_objects_ = create_subscription( "~/input/objects", rclcpp::SensorDataQoS(), @@ -430,13 +430,13 @@ bool PredictedPathCheckerNode::isItDiscretePoint( const auto nearest_point = utils::calcInterpolatedPoint( reference_trajectory, collision_point.pose.position, *nearest_segment, false); - const auto distance = tier4_autoware_utils::calcDistance2d( + const auto distance = autoware_universe_utils::calcDistance2d( nearest_point.pose.position, collision_point.pose.position); const auto yaw_diff = - std::abs(tier4_autoware_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + std::abs(autoware_universe_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); return distance >= node_param_.distinct_point_distance_threshold || - yaw_diff >= tier4_autoware_utils::deg2rad(node_param_.distinct_point_yaw_threshold); + yaw_diff >= autoware_universe_utils::deg2rad(node_param_.distinct_point_yaw_threshold); } Trajectory PredictedPathCheckerNode::cutTrajectory( @@ -452,8 +452,8 @@ Trajectory PredictedPathCheckerNode::cutTrajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware_universe_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware_universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -526,7 +526,7 @@ std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; const auto acceleration_obj = object.kinematics.initial_acceleration_with_covariance.accel.linear.x; - const auto k = std::cos(tier4_autoware_utils::normalizeRadian( + const auto k = std::cos(autoware_universe_utils::normalizeRadian( tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); const auto projected_velocity = velocity_obj * k; const auto projected_acceleration = acceleration_obj * k; @@ -555,8 +555,8 @@ void PredictedPathCheckerNode::filterObstacles( const double max_length = utils::calcObstacleMaxLength(object.shape); const size_t seg_idx = motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); - const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1)); + const auto p_front = autoware_universe_utils::getPoint(traj.at(seg_idx)); + const auto p_back = autoware_universe_utils::getPoint(traj.at(seg_idx + 1)); const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.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 a9c3e2b24f9a5..1321f1d5af50a 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 @@ -21,10 +21,10 @@ namespace utils { +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::getRPY; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getRPY; // Utils Functions Polygon2d createOneStepPolygon( @@ -38,40 +38,42 @@ Polygon2d createOneStepPolygon( const double rear_overhang = vehicle_info.rear_overhang_m; { // base step - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) - .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) + .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); } { // next step - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) - .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) + .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); } - polygon = tier4_autoware_utils::isClockwise(polygon) + polygon = autoware_universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware_universe_utils::inverseClockwise(polygon); Polygon2d hull_polygon; boost::geometry::convex_hull(polygon, hull_polygon); @@ -95,8 +97,8 @@ TrajectoryPoint calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = trajectory.at(segment_idx); const auto & next_pt = trajectory.at(segment_idx + 1); - const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_point); + const auto v1 = autoware_universe_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = autoware_universe_utils::point2tfVector(curr_pt, target_point); if (v1.length2() < 1e-3) { return curr_pt; } @@ -109,7 +111,7 @@ TrajectoryPoint calcInterpolatedPoint( // pose interpolation interpolated_point.pose = - tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + autoware_universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -175,7 +177,7 @@ std::pair findStopPoint( base_point.pose.position.y - next_point.pose.position.y)); geometry_msgs::msg::Pose interpolated_pose = - tier4_autoware_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); + autoware_universe_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); TrajectoryPoint output; output.set__pose(interpolated_pose); return std::make_pair(stop_segment_idx, output); @@ -247,7 +249,7 @@ double getNearestPointAndDistanceForPredictedObject( bool is_init = false; for (const auto & p : points) { - double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); + double norm = autoware_universe_utils::calcDistance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; *nearest_collision_point = p; @@ -369,7 +371,7 @@ Polygon2d convertObjToPolygon(const PredictedObject & obj) bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) { - const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z; + const auto yaw = autoware_universe_utils::getRPY(ego_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); const Eigen::Vector2d obstacle_vec( obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); @@ -401,7 +403,7 @@ void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) { - const double yaw = tier4_autoware_utils::getRPY( + const double yaw = autoware_universe_utils::getRPY( predicted_object.kinematics.initial_pose_with_covariance.pose.orientation) .z; const double vx = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; @@ -412,8 +414,8 @@ void getCurrentObjectPose( const double delta_yaw = predicted_object.kinematics.initial_twist_with_covariance.twist.angular.z * dt; geometry_msgs::msg::Transform transform; - transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); - transform.rotation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + transform.translation = autoware_universe_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); tf2::Transform tf_pose; tf2::Transform tf_offset; @@ -422,8 +424,8 @@ void getCurrentObjectPose( tf2::toMsg(tf_pose * tf_offset, predicted_object.kinematics.initial_pose_with_covariance.pose); predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x += ax * dt; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY( - 0.0, 0.0, tier4_autoware_utils::normalizeRadian(yaw + delta_yaw)); + autoware_universe_utils::createQuaternionFromRPY( + 0.0, 0.0, autoware_universe_utils::normalizeRadian(yaw + delta_yaw)); } } // namespace utils diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 1cfb3e12d2a06..efeb068fca115 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -17,8 +17,8 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include #include -#include #include #include @@ -72,9 +72,9 @@ class controlEvaluatorNode : public rclcpp::Node // onDiagnostics(). rclcpp::Subscription::SharedPtr control_diag_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber odometry_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber traj_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 176537867c600..c76b27b006342 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -15,12 +15,12 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils diagnostic_msgs motion_utils pluginlib rclcpp rclcpp_components - tier4_autoware_utils ament_cmake_ros diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index c5eb73978d30b..d7c9bbb1308f9 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -14,9 +14,9 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" namespace control_diagnostics { @@ -28,13 +28,13 @@ double calcLateralDeviation(const Trajectory & traj, const Point & point) { const size_t nearest_index = motion_utils::findNearestIndex(traj.points, point); return std::abs( - tier4_autoware_utils::calcLateralDeviation(traj.points[nearest_index].pose, point)); + autoware_universe_utils::calcLateralDeviation(traj.points[nearest_index].pose, point)); } double calcYawDeviation(const Trajectory & traj, const Pose & pose) { const size_t nearest_index = motion_utils::findNearestIndex(traj.points, pose.position); - return std::abs(tier4_autoware_utils::calcYawDeviation(traj.points[nearest_index].pose, pose)); + return std::abs(autoware_universe_utils::calcYawDeviation(traj.points[nearest_index].pose, pose)); } } // namespace metrics diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 0743a5e9d1086..53502b5353956 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -15,6 +15,7 @@ autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs @@ -25,7 +26,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 280d8f085df96..8935b41aa5bc6 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -14,9 +14,9 @@ #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" namespace planning_diagnostics { @@ -38,8 +38,8 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra */ for (TrajectoryPoint p : traj.points) { const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); - stat.add( - tier4_autoware_utils::calcLateralDeviation(ref.points[nearest_index].pose, p.pose.position)); + stat.add(autoware_universe_utils::calcLateralDeviation( + ref.points[nearest_index].pose, p.pose.position)); } return stat; } @@ -57,7 +57,7 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) */ for (TrajectoryPoint p : traj.points) { const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); - stat.add(tier4_autoware_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); + stat.add(autoware_universe_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); } return stat; } @@ -81,21 +81,21 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) { Stat stat; - stat.add(std::abs(tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point))); + stat.add(std::abs(autoware_universe_utils::calcLongitudinalDeviation(base_pose, target_point))); return stat; } Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point) { Stat stat; - stat.add(std::abs(tier4_autoware_utils::calcLateralDeviation(base_pose, target_point))); + stat.add(std::abs(autoware_universe_utils::calcLateralDeviation(base_pose, target_point))); return stat; } Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose) { Stat stat; - stat.add(std::abs(tier4_autoware_utils::calcYawDeviation(base_pose, target_pose))); + stat.add(std::abs(autoware_universe_utils::calcYawDeviation(base_pose, target_pose))); return stat; } } // namespace metrics diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 75744a4ca3cb8..906501b05e171 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics @@ -22,7 +22,7 @@ namespace utils { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::calcDistance2d; +using autoware_universe_utils::calcDistance2d; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index 3303848f11e3f..7836b0b6d694a 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -14,7 +14,7 @@ #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -28,7 +28,7 @@ namespace planning_diagnostics namespace metrics { using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::calcDistance2d; +using autoware_universe_utils::calcDistance2d; Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index cea2a1c29228d..8be0280bb38f5 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -41,7 +41,7 @@ Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & tr for (size_t i = 0; i < traj1.points.size(); ++i) { for (size_t j = 0; j < traj2.points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d(traj1.points[i], traj2.points[j]); + const double dist = autoware_universe_utils::calcDistance2d(traj1.points[i], traj2.points[j]); if (i > 0 && j > 0) { ca(i, j) = std::max(std::min(ca(i - 1, j), std::min(ca(i - 1, j - 1), ca(i, j - 1))), dist); } else if (i > 0 /*&& j == 0*/) { @@ -64,7 +64,7 @@ Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & tr return stat; } for (const auto & point : traj2.points) { - const auto p0 = tier4_autoware_utils::getPoint(point); + const auto p0 = autoware_universe_utils::getPoint(point); // find nearest segment const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(traj1.points, p0); double dist; @@ -72,19 +72,19 @@ Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & tr if ( nearest_segment_idx == traj1.points.size() - 2 && motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) > - tier4_autoware_utils::calcDistance2d( + autoware_universe_utils::calcDistance2d( traj1.points[nearest_segment_idx], traj1.points[nearest_segment_idx + 1])) { // distance to last point - dist = tier4_autoware_utils::calcDistance2d(traj1.points.back(), p0); + dist = autoware_universe_utils::calcDistance2d(traj1.points.back(), p0); } else if ( // NOLINT nearest_segment_idx == 0 && motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) <= 0) { // distance to first point - dist = tier4_autoware_utils::calcDistance2d(traj1.points.front(), p0); + dist = autoware_universe_utils::calcDistance2d(traj1.points.front(), p0); } else { // orthogonal distance - const auto p1 = tier4_autoware_utils::getPoint(traj1.points[nearest_segment_idx]); - const auto p2 = tier4_autoware_utils::getPoint(traj1.points[nearest_segment_idx + 1]); + const auto p1 = autoware_universe_utils::getPoint(traj1.points[nearest_segment_idx]); + const auto p2 = autoware_universe_utils::getPoint(traj1.points[nearest_segment_idx + 1]); dist = std::abs((p2.x - p1.x) * (p1.y - p0.y) - (p1.x - p0.x) * (p2.y - p1.y)) / std::sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y)); } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index c7e0fc5f44cd9..3f33447c1ce6b 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -15,13 +15,13 @@ #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/planning_evaluator/metrics/metrics_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics { -using tier4_autoware_utils::calcCurvature; -using tier4_autoware_utils::calcDistance2d; +using autoware_universe_utils::calcCurvature; +using autoware_universe_utils::calcDistance2d; Stat calcTrajectoryInterval(const Trajectory & traj) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 1f689d9fe251a..7d2a82f7bc526 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -18,8 +18,8 @@ #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( @@ -131,7 +131,7 @@ Trajectory MetricsCalculator::getLookaheadTrajectory( auto prev_point_it = curr_point_it; while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { lookahead_traj.points.push_back(*curr_point_it); - const auto d = tier4_autoware_utils::calcDistance2d( + const auto d = autoware_universe_utils::calcDistance2d( prev_point_it->pose.position, curr_point_it->pose.position); dist += d; if (prev_point_it->longitudinal_velocity_mps != 0.0) { diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml index 1b62b06e57c9f..3642e0cf94604 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/kinematic_evaluator/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs @@ -26,7 +27,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_gtest ament_lint_auto diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml index ac52f9fe94d5e..8d3b788a2b8d9 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs @@ -22,7 +23,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_gtest ament_lint_auto 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 b05bb5e1d8ad3..2d60ef826a5e9 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 @@ -31,10 +31,10 @@ namespace marker_utils { using autoware_perception_msgs::msg::PredictedObject; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index d64d3a5ec4957..ac8a8d215862d 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_universe_utils autoware_vehicle_info_utils diagnostic_msgs eigen @@ -32,7 +33,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp index 047d278334eb1..437e53dbbf1e4 100644 --- a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp @@ -14,17 +14,17 @@ #include "perception_online_evaluator/metrics/detection_count.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "perception_online_evaluator/utils/objects_filtering.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include namespace perception_diagnostics { namespace metrics { -using tier4_autoware_utils::toHexString; +using autoware_universe_utils::toHexString; bool isCountObject( const std::uint8_t classification, const std::unordered_map & params) diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp index 7a9435444c065..ed3f74658594d 100644 --- a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -14,8 +14,8 @@ #include "perception_online_evaluator/metrics/deviation_metrics.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" #include @@ -32,7 +32,7 @@ double calcLateralDeviation(const std::vector & ref_path, const Pose & tar const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); return std::abs( - tier4_autoware_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); + autoware_universe_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); } double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose) @@ -42,7 +42,7 @@ double calcYawDeviation(const std::vector & ref_path, const Pose & target_ } const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); - return std::abs(tier4_autoware_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); + return std::abs(autoware_universe_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); } std::vector calcPredictedPathDeviation( @@ -56,7 +56,7 @@ std::vector calcPredictedPathDeviation( for (const Pose & p : pred_path.path) { const size_t nearest_index = motion_utils::findNearestIndex(ref_path, p.position); deviations.push_back( - tier4_autoware_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); + autoware_universe_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); } return deviations; diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index cc455445ca9f8..55f459582a25d 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -14,17 +14,17 @@ #include "perception_online_evaluator/metrics_calculator.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/object_classification.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include namespace perception_diagnostics { +using autoware_universe_utils::inverseTransformPoint; using object_recognition_utils::convertLabelToString; -using tier4_autoware_utils::inverseTransformPoint; std::optional MetricsCalculator::calculate(const Metric & metric) const { @@ -238,7 +238,7 @@ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( Stat stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware_universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -267,7 +267,7 @@ MetricStatMap MetricsCalculator::calcYawDeviationMetrics( Stat stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware_universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -326,7 +326,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto stamp = objects.header.stamp; for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware_universe_utils::toHexString(object.object_id); const auto predicted_paths = object.kinematics.predicted_paths; for (size_t i = 0; i < predicted_paths.size(); i++) { const auto predicted_path = predicted_paths[i]; @@ -350,7 +350,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto history_pose = history_object.kinematics.initial_pose_with_covariance.pose; const Pose & p = predicted_path.path[j]; const double distance = - tier4_autoware_utils::calcDistance2d(p.position, history_pose.position); + autoware_universe_utils::calcDistance2d(p.position, history_pose.position); deviation_map_for_paths[uuid][i].push_back(distance); // Save debug information @@ -424,7 +424,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware_universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -444,7 +444,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); // Calculate the absolute difference between current_yaw and previous_yaw const double yaw_diff = - std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw)); + std::abs(autoware_universe_utils::normalizeRadian(current_yaw - previous_yaw)); // The yaw difference is flipped if the angle is larger than 90 degrees const double yaw_diff_flip_fixed = std::min(yaw_diff, M_PI - yaw_diff); const double yaw_rate = yaw_diff_flip_fixed / time_diff; @@ -494,7 +494,7 @@ void MetricsCalculator::setPredictedObjects( // store objects to check deviation { - using tier4_autoware_utils::toHexString; + using autoware_universe_utils::toHexString; for (const auto & object : objects.objects) { std::string uuid = toHexString(object.object_id); updateObjects(uuid, current_stamp_, object); @@ -557,7 +557,7 @@ void MetricsCalculator::updateHistoryPath() const auto current_pose = object.kinematics.initial_pose_with_covariance.pose; const auto prev_pose = prev_object.kinematics.initial_pose_with_covariance.pose; const auto velocity = - tier4_autoware_utils::calcDistance2d(current_pose.position, prev_pose.position) / + autoware_universe_utils::calcDistance2d(current_pose.position, prev_pose.position) / time_diff; if (velocity < parameters_->stopped_velocity_threshold) { continue; @@ -603,9 +603,9 @@ std::vector MetricsCalculator::generateHistoryPathWithPrev( std::vector MetricsCalculator::averageFilterPath( const std::vector & path, const size_t window_size) const { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createQuaternionFromYaw; + using autoware_universe_utils::calcAzimuthAngle; + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::createQuaternionFromYaw; std::vector filtered_path; filtered_path.reserve(path.size()); // Reserve space to avoid reallocations @@ -645,7 +645,7 @@ std::vector MetricsCalculator::averageFilterPath( if (i > 0) { const double azimuth = calcAzimuthAngle(path.at(i - 1).position, path.at(i).position); const double yaw = tf2::getYaw(path.at(i).orientation); - if (tier4_autoware_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { + if (autoware_universe_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { continue; } } @@ -662,7 +662,7 @@ std::vector MetricsCalculator::averageFilterPath( const double azimuth_to_prev = calcAzimuthAngle((it - 2)->position, (it - 1)->position); const double azimuth_to_current = calcAzimuthAngle((it - 1)->position, it->position); if ( - std::abs(tier4_autoware_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > + std::abs(autoware_universe_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > M_PI_2) { it = filtered_path.erase(it); continue; diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 2933afdab3d08..56b79eb3acf62 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -14,12 +14,12 @@ #include "perception_online_evaluator/perception_online_evaluator_node.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "perception_online_evaluator/utils/marker_utils.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include +#include #include "boost/lexical_cast.hpp" @@ -156,7 +156,7 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - tier4_autoware_utils::appendMarkerArray(added, &marker); + autoware_universe_utils::appendMarkerArray(added, &marker); }; const auto & p = parameters_->debug_marker_parameters; @@ -236,7 +236,7 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParameter( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto & p = parameters_; @@ -305,8 +305,8 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame void PerceptionOnlineEvaluatorNode::initParameter() { - using tier4_autoware_utils::getOrDeclareParameter; - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::getOrDeclareParameter; + using autoware_universe_utils::updateParam; auto & p = parameters_; diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index eddeadc331101..6085ea0b6ef49 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -14,11 +14,11 @@ #include "perception_online_evaluator/utils/marker_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -28,13 +28,13 @@ namespace marker_utils { +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; void addFootprintMarker( @@ -46,13 +46,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -167,7 +167,7 @@ std_msgs::msg::ColorRGBA createColorFromString(const std::string & str) const auto r = (hash & 0xFF) / 255.0; const auto g = ((hash >> 8) & 0xFF) / 255.0; const auto b = ((hash >> 16) & 0xFF) / 255.0; - return tier4_autoware_utils::createMarkerColor(r, g, b, 0.5); + return autoware_universe_utils::createMarkerColor(r, g, b, 0.5); } MarkerArray createObjectPolygonMarkerArray( @@ -182,7 +182,7 @@ MarkerArray createObjectPolygonMarkerArray( const double z = object.kinematics.initial_pose_with_covariance.pose.position.z; const double height = object.shape.dimensions.z; - const auto polygon = tier4_autoware_utils::toPolygon2d( + const auto polygon = autoware_universe_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); for (const auto & p : polygon.outer()) { marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2)); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 3db487f3167ae..a369d36882c9d 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -16,8 +16,8 @@ #include "rclcpp/time.hpp" #include +#include #include -#include #include #include @@ -43,7 +43,7 @@ using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification using nav_msgs::msg::Odometry; using TFMessage = tf2_msgs::msg::TFMessage; -using tier4_autoware_utils::generateUUID; +using autoware_universe_utils::generateUUID; constexpr double epsilon = 1e-6; diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 0ea40690d9f4d..f8781ada3d38d 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -62,7 +62,7 @@ #include #endif -#include +#include ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : rclcpp::Node("ar_tag_based_localizer", options), cam_info_received_(false) @@ -185,7 +185,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) pose_array_msg.header.frame_id = "map"; for (const Landmark & landmark : landmarks) { const Pose detected_marker_on_map = - tier4_autoware_utils::transformPose(landmark.pose, self_pose); + autoware_universe_utils::transformPose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } detected_tag_pose_pub_->publish(pose_array_msg); @@ -194,7 +194,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) // calc new_self_pose const Pose new_self_pose = landmark_manager_.calculate_new_self_pose(landmarks, self_pose, consider_orientation_); - const Pose diff_pose = tier4_autoware_utils::inverseTransformPose(new_self_pose, self_pose); + const Pose diff_pose = autoware_universe_utils::inverseTransformPose(new_self_pose, self_pose); const double distance = std::hypot(diff_pose.position.x, diff_pose.position.y, diff_pose.position.z); diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index 3953b3ba130d9..b2c4a7dd6298a 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -14,9 +14,9 @@ #include "autoware/landmark_manager/landmark_manager.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "localization_util/util_func.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -161,7 +161,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // convert base_link to map const Pose detected_landmark_on_map = - tier4_autoware_utils::transformPose(detected_landmark_on_base_link, self_pose); + autoware_universe_utils::transformPose(detected_landmark_on_base_link, self_pose); // match to map if (landmarks_map_.count(landmark.id) == 0) { @@ -171,7 +171,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // check all poses for (const Pose mapped_landmark_on_map : landmarks_map_.at(landmark.id)) { // check distance - const double curr_distance = tier4_autoware_utils::calcDistance3d( + const double curr_distance = autoware_universe_utils::calcDistance3d( mapped_landmark_on_map.position, detected_landmark_on_map.position); if (curr_distance > min_distance) { continue; diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index b78e9b1ee0469..ce1d7a95178ee 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -20,10 +20,10 @@ #include "ekf_localizer/hyper_parameters.hpp" #include "ekf_localizer/warning.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -142,7 +142,7 @@ class EKFLocalizer : public rclcpp::Node std::shared_ptr tf_br_; //!< @brief logger configure module - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; //!< @brief extended kalman filter instance. std::unique_ptr ekf_module_; @@ -236,7 +236,7 @@ class EKFLocalizer : public rclcpp::Node const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); - tier4_autoware_utils::StopWatch stop_watch_; + autoware_universe_utils::StopWatch stop_watch_; friend class EKFLocalizerTestSuite; // for test code }; diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index dad1b0e730711..07efca6339b98 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -23,6 +23,7 @@ eigen + autoware_universe_utils diagnostic_msgs fmt geometry_msgs @@ -33,7 +34,6 @@ std_srvs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/localization/ekf_localizer/src/covariance.cpp b/localization/ekf_localizer/src/covariance.cpp index fc16abf429d93..4e68de981e196 100644 --- a/localization/ekf_localizer/src/covariance.cpp +++ b/localization/ekf_localizer/src/covariance.cpp @@ -14,10 +14,10 @@ #include "ekf_localizer/covariance.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "ekf_localizer/state_index.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" -using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) { diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 8e7121ec73a12..5400c0a70b6f3 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -18,11 +18,11 @@ #include "ekf_localizer/string.hpp" #include "ekf_localizer/warning_message.hpp" +#include +#include +#include #include #include -#include -#include -#include #include @@ -96,7 +96,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) std::shared_ptr(this, [](auto) {})); ekf_module_ = std::make_unique(warning_, params_); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); z_filter_.set_proc_dev(params_.z_filter_proc_dev); roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); @@ -264,7 +264,7 @@ void EKFLocalizer::timer_tf_callback() const rclcpp::Time current_time = this->now(); geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = tier4_autoware_utils::pose2transform( + transform_stamped = autoware_universe_utils::pose2transform( ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link"); transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); @@ -436,9 +436,9 @@ void EKFLocalizer::update_simple_1d_filters( { double z = pose.pose.pose.position.z; - const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware_universe_utils::getRPY(pose.pose.pose.orientation); - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); double pitch_dev = @@ -454,9 +454,9 @@ void EKFLocalizer::init_simple_1d_filters( { double z = pose.pose.pose.position.z; - const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware_universe_utils::getRPY(pose.pose.pose.orientation); - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 8703d8754eaaf..82e01f6b81065 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -22,8 +22,8 @@ #include "ekf_localizer/state_transition.hpp" #include "ekf_localizer/warning_message.hpp" -#include -#include +#include +#include #include #include @@ -65,7 +65,7 @@ void EKFModule::initialize( x(IDX::VX) = 0.0; x(IDX::WZ) = 0.0; - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; @@ -97,13 +97,13 @@ geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; - current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z); + current_ekf_pose.pose.position = autoware_universe_utils::createPoint(x, y, z); if (get_biased_yaw) { current_ekf_pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); + autoware_universe_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); } else { current_ekf_pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + autoware_universe_utils::createQuaternionFromRPY(roll, pitch, yaw); } return current_ekf_pose; } @@ -285,7 +285,7 @@ bool EKFModule::measurement_update_pose( geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time) { - const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware_universe_utils::getRPY(pose.pose.pose.orientation); const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); PoseWithCovariance pose_with_z_delay; pose_with_z_delay = pose; diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index 4533bacbee991..fbc177d602ff0 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -14,8 +14,8 @@ #include "ekf_localizer/measurement.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "ekf_localizer/state_index.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" Eigen::Matrix pose_measurement_matrix() { @@ -38,7 +38,7 @@ Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix3d r; - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); @@ -49,7 +49,7 @@ Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix2d r; - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_YAW); return r * static_cast(smoothing_step); diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index f85640ab2c523..eb1db06b72998 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,10 +15,10 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "gyro_odometer/diagnostics_module.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -43,7 +43,7 @@ namespace autoware::gyro_odometer class GyroOdometerNode : public rclcpp::Node { private: - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); @@ -67,8 +67,8 @@ class GyroOdometerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; - std::shared_ptr transform_listener_; - std::unique_ptr logger_configure_; + std::shared_ptr transform_listener_; + std::unique_ptr logger_configure_; std::string output_frame_; double message_timeout_sec_; diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 575f6a2d74837..faa64a1313ebb 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils diagnostic_msgs fmt geometry_msgs @@ -25,7 +26,6 @@ sensor_msgs tf2 tf2_geometry_msgs - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 0ec479770740f..0878c44ffa2cb 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -33,7 +33,7 @@ namespace autoware::gyro_odometer std::array transform_covariance(const std::array & cov) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); @@ -52,8 +52,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_arrived_(false), imu_arrived_(false) { - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, @@ -208,8 +208,8 @@ void GyroOdometerNode::concat_gyro_and_odometer() gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } - using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX_XYZ = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // calc mean, covariance double vx_mean = 0; diff --git a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 53f243520742f..12f0a1141d199 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -16,8 +16,8 @@ #define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ #include +#include #include -#include #include #include @@ -43,7 +43,7 @@ class LocalizationErrorMonitor : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; double scale_; double error_ellipse_size_; diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 0138451b069e4..681da7e57c6c5 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -20,13 +20,13 @@ autoware_cmake eigen + autoware_universe_utils diagnostic_msgs diagnostic_updater nav_msgs rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp index 04bb37a85e41b..51fc1607d048a 100644 --- a/localization/localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -55,7 +55,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o diag_pub_ = this->create_publisher("/diagnostics", 10); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker( diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml index fa8da2aa1231a..bb1ca3123ad27 100644 --- a/localization/localization_util/package.xml +++ b/localization/localization_util/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils fmt geometry_msgs nav_msgs @@ -29,7 +30,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 2bcb6f4cf6fb8..ac913b128b5f1 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -20,9 +20,9 @@ #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/particle.hpp" +#include +#include #include -#include -#include #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index cbd2797c57922..5a28ed7eb8a5e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -22,8 +22,8 @@ #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" +#include #include -#include #include #include @@ -203,7 +203,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr diagnostics_ndt_align_; std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; HyperParameters param_; }; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 6b0a44bdf1c55..c69836d77c825 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_map_msgs + autoware_universe_utils diagnostic_msgs fmt geometry_msgs @@ -37,7 +38,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 31f58f94ed6e8..f392ba4b3c19f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -19,8 +19,8 @@ #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" -#include -#include +#include +#include #include @@ -210,7 +210,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) diagnostics_trigger_node_ = std::make_unique(this, "trigger_node_service_status"); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void NDTScanMatcher::callback_timer() @@ -615,7 +615,7 @@ bool NDTScanMatcher::callback_sensor_points_main( pcl::shared_ptr> sensor_points_in_map_ptr( new pcl::PointCloud); - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); @@ -671,10 +671,10 @@ void NDTScanMatcher::transform_sensor_measurement( } const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - tier4_autoware_utils::transform2pose(transform); + autoware_universe_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); } @@ -686,7 +686,7 @@ void NDTScanMatcher::publish_tf( result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); + autoware_universe_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -730,7 +730,7 @@ void NDTScanMatcher::publish_marker( marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); + marker.scale = autoware_universe_utils::createMarkerScale(0.3, 0.1, 0.1); int i = 0; marker.ns = "result_pose_matrix_array"; marker.action = visualization_msgs::msg::Marker::ADD; @@ -759,7 +759,7 @@ void NDTScanMatcher::publish_initial_to_result( { geometry_msgs::msg::PoseStamped initial_to_result_relative_pose_stamped; initial_to_result_relative_pose_stamped.pose = - tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); + autoware_universe_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); @@ -1091,7 +1091,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); auto sensor_points_in_map_ptr = std::make_shared>(); - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud( initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp index 0d8880d5c4d16..0866234383727 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp @@ -14,7 +14,7 @@ #include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" -#include +#include #include @@ -28,9 +28,9 @@ PcdMapBasedRule::PcdMapBasedRule( shared_data_(std::move(shared_data)) { const int pcd_density_upper_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); + autoware_universe_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); const int pcd_density_lower_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); + autoware_universe_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); pcd_occupancy_ = std::make_unique( pcd_density_upper_threshold, pcd_density_lower_threshold); diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 134cad7534502..4880e048ea392 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -19,6 +19,7 @@ autoware_adapi_v1_msgs autoware_map_msgs + autoware_universe_utils diagnostic_msgs geometry_msgs lanelet2_extension @@ -30,7 +31,6 @@ sensor_msgs std_msgs std_srvs - tier4_autoware_utils ublox_msgs visualization_msgs yabloc_particle_filter diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp index fda77ec877482..4228ade6be701 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -19,8 +19,8 @@ #include "pose_estimator_arbiter/stopper/base_stopper.hpp" #include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include #include -#include #include #include @@ -53,7 +53,7 @@ class PoseEstimatorArbiter : public rclcpp::Node // Set of running pose estimators specified by ros param `pose_sources` const std::unordered_set running_estimator_list_; // Configuration to allow changing the log level by service - const std::unique_ptr logger_configure_; + const std::unique_ptr logger_configure_; // This is passed to several modules (stoppers & rule) so that all modules can access common data // without passing them as arguments. Also, modules can register subscriber callbacks through diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp index 67c555227976d..4d655875ba325 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -46,7 +46,7 @@ PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_estimator_arbiter", options), running_estimator_list_(parse_estimator_name_args( declare_parameter>("pose_sources"), get_logger())), - logger_configure_(std::make_unique(this)) + logger_configure_(std::make_unique(this)) { // Shared data shared_data_ = std::make_shared(); diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 58968828bc2fa..8de8a8ac9a1e1 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -19,6 +19,7 @@ ament_cmake autoware_cmake + autoware_universe_utils component_interface_specs component_interface_utils geometry_msgs @@ -27,7 +28,6 @@ rclcpp rclcpp_components std_srvs - tier4_autoware_utils tier4_localization_msgs ament_cmake_cppcheck diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 1111547b393e7..1e351f8a90d7b 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -56,7 +56,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) stop_check_duration_ = declare_parameter("stop_check_duration"); stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index d80fb26fdd41f..3a0896a07ba98 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -15,10 +15,10 @@ #ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ +#include #include #include #include -#include #include @@ -55,7 +55,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr stop_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index d658d1c2d437f..04e9747d3723b 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -18,6 +18,7 @@ autoware_cmake ament_index_cpp + autoware_universe_utils diagnostic_msgs geometry_msgs nav_msgs @@ -25,7 +26,6 @@ rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils tier4_debug_msgs ament_cmake_cppcheck diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index 797f18bba3ea9..436fd4eee7692 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -14,7 +14,7 @@ #include "autoware/pose_instability_detector/pose_instability_detector.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -123,7 +123,7 @@ void PoseInstabilityDetector::callback_timer() // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_dr = tier4_autoware_utils::inverseTransformPose(*dr_pose, latest_ekf_pose); + const Pose ekf_to_dr = autoware_universe_utils::inverseTransformPose(*dr_pose, latest_ekf_pose); const geometry_msgs::msg::Point pos = ekf_to_dr.position; const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_dr.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index d4c8b71fa6b66..18038d80fbfca 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -17,6 +17,7 @@ rosidl_default_generators autoware_map_msgs + autoware_universe_utils cv_bridge geometry_msgs lanelet2_core @@ -29,7 +30,6 @@ sophus std_msgs tf2_ros - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index fb7bc53c05e5e..aa0244e5d7e90 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -14,8 +14,8 @@ #include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" +#include #include -#include #include #include @@ -198,7 +198,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = tier4_autoware_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = autoware_universe_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -228,7 +228,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = tier4_autoware_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = autoware_universe_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 416acfdc76a16..453ae58b9b33b 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -16,13 +16,13 @@ ament_cmake autoware_cmake + autoware_universe_utils cv_bridge pcl_conversions rclcpp rclcpp_components sensor_msgs std_msgs - tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index 4cc2a9de4acc7..f319fcc9fd77a 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -15,9 +15,9 @@ #include "yabloc_image_processing/graph_segment/graph_segment.hpp" #include "yabloc_image_processing/graph_segment/histogram.hpp" +#include #include #include -#include #include #include @@ -98,7 +98,7 @@ void GraphSegment::on_image(const Image & msg) cv::resize(image, resized, cv::Size(), 0.5, 0.5); // Execute graph-based segmentation - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; cv::Mat segmented; segmentation_->processImage(resized, segmented); RCLCPP_INFO_STREAM(get_logger(), "segmentation time: " << stop_watch.toc() * 1000 << "[ms]"); diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 9854fa9ab7106..f0f451834c66e 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -14,8 +14,8 @@ #include "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" +#include #include -#include #include #include @@ -53,7 +53,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st cv::Mat lines; { - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; line_segment_detector_->detect(gray_image, lines); if (lines.size().width != 0) { line_segment_detector_->drawSegments(gray_image, lines); diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index c4b868289eb16..ad38c320fc510 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include -#include #include #include @@ -147,7 +147,7 @@ class UndistortNode : public rclcpp::Node sub_compressed_image_.reset(); } - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } @@ -161,7 +161,7 @@ class UndistortNode : public rclcpp::Node make_remap_lut(); } - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index bec77403f231a..8e2b98d10931b 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -17,6 +17,7 @@ autoware_cmake rosidl_default_generators + autoware_universe_utils geometry_msgs rclcpp rclcpp_components @@ -27,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp index e4c3065fd3efb..35113b4c5af36 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp @@ -15,9 +15,9 @@ #include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" #include "yabloc_particle_filter/camera_corrector/logit.hpp" +#include +#include #include -#include -#include #include #include #include @@ -130,7 +130,7 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments_msg) { - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; const rclcpp::Time stamp = line_segments_msg.header.stamp; std::optional opt_array = this->get_synchronized_particle_array(stamp); if (!opt_array.has_value()) { @@ -258,7 +258,7 @@ float abs_cos(const Eigen::Vector3f & t, float deg) { const float radian = deg * M_PI / 180.0; Eigen::Vector2f x(t.x(), t.y()); - Eigen::Vector2f y(tier4_autoware_utils::cos(radian), tier4_autoware_utils::sin(radian)); + Eigen::Vector2f y(autoware_universe_utils::cos(radian), autoware_universe_utils::sin(radian)); x.normalize(); return std::abs(x.dot(y)); } diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index 62d8ed826b373..69f3642c8eff6 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -16,9 +16,9 @@ #include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp" +#include #include #include -#include #include @@ -169,7 +169,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = tier4_autoware_utils::createMarkerColor(0, 0, 1.0f, 1.0f); + marker.color = autoware_universe_utils::createMarkerColor(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp index de53d78d7f782..452805803ffa6 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ #define AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ +#include +#include #include -#include -#include #include #include @@ -41,8 +41,8 @@ namespace autoware::crosswalk_traffic_light_estimator using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; -using tier4_autoware_utils::DebugPublisher; -using tier4_autoware_utils::StopWatch; +using autoware_universe_utils::DebugPublisher; +using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/package.xml b/perception/autoware_crosswalk_traffic_light_estimator/package.xml index d844ea27b52b5..18b0b8a41264f 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/package.xml +++ b/perception/autoware_crosswalk_traffic_light_estimator/package.xml @@ -15,10 +15,10 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils lanelet2_extension rclcpp rclcpp_components - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 27c2a5079f441..86633721c936b 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -15,18 +15,18 @@ #ifndef MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "map_based_prediction/path_generator.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include #include #include @@ -129,7 +129,7 @@ using autoware_perception_msgs::msg::TrafficLightElement; using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::StopWatch; +using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node @@ -143,12 +143,12 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_markers_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; - tier4_autoware_utils::InterProcessPollingSubscriber sub_traffic_signals_{ - this, "/traffic_signals"}; + autoware_universe_utils::InterProcessPollingSubscriber + sub_traffic_signals_{this, "/traffic_signals"}; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; // Object History std::unordered_map> road_users_history; @@ -169,7 +169,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::vector & parameters); // Pose Transform Listener - tier4_autoware_utils::TransformListener transform_listener_{this}; + autoware_universe_utils::TransformListener transform_listener_{this}; // Path Generator std::shared_ptr path_generator_; @@ -221,7 +221,7 @@ class MapBasedPredictionNode : public rclcpp::Node bool match_lost_and_appeared_crosswalk_users_; bool remember_lost_crosswalk_users_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // Member Functions void mapCallback(const LaneletMapBin::ConstSharedPtr msg); @@ -314,8 +314,8 @@ class MapBasedPredictionNode : public rclcpp::Node inline std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using tier4_autoware_utils::calcCurvature; - using tier4_autoware_utils::getPoint; + using autoware_universe_utils::calcCurvature; + using autoware_universe_utils::getPoint; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 3671c1a01b0a2..caf6b83936faf 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -15,6 +15,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils glog interpolation lanelet2_extension @@ -24,7 +25,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs unique_identifier_msgs visualization_msgs diff --git a/perception/autoware_map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp index 0c58d4aae1638..638c10c1a3b1a 100644 --- a/perception/autoware_map_based_prediction/src/debug.cpp +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "map_based_prediction/map_based_prediction_node.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace autoware::map_based_prediction { @@ -29,7 +29,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( marker.type = visualization_msgs::msg::Marker::CUBE; marker.action = visualization_msgs::msg::Marker::ADD; marker.pose = object.kinematics.pose_with_covariance.pose; - marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0); + marker.scale = autoware_universe_utils::createMarkerScale(3.0, 1.0, 1.0); // Color by maneuver double r = 0.0; @@ -42,7 +42,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( } else { b = 1.0; } - marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.8); + marker.color = autoware_universe_utils::createMarkerColor(r, g, b, 0.8); return marker; } diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 20d532d7127d7..a350873321ce9 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,16 +14,16 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include -#include #include @@ -92,7 +92,7 @@ double calcAbsLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware_universe_utils::createPoint(x, y, 0.0); } return std::fabs(motion_utils::calcLateralOffset(boundary_path, search_pose.position)); @@ -218,7 +218,7 @@ double calcAbsYawDiffBetweenLaneletAndObject( const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); return abs_norm_delta; } @@ -418,7 +418,7 @@ boost::optional isReachableCrosswalkEdgePoints( const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware_universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { @@ -529,7 +529,7 @@ bool hasPotentialToReach( { const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware_universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; constexpr double stop_velocity_th = 0.14; // [m/s] const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); @@ -547,16 +547,16 @@ bool hasPotentialToReach( const double pedestrian_to_crosswalk_left_direction = std::atan2(left_point.y() - obj_pos.y, left_point.x() - obj_pos.x); return std::make_pair( - tier4_autoware_utils::normalizeRadian( + autoware_universe_utils::normalizeRadian( pedestrian_to_crosswalk_right_direction - pedestrian_to_crosswalk_center_direction), - tier4_autoware_utils::normalizeRadian( + autoware_universe_utils::normalizeRadian( pedestrian_to_crosswalk_left_direction - pedestrian_to_crosswalk_center_direction)); }(); const double pedestrian_heading_rel_direction = [&]() { const double pedestrian_heading_direction = std::atan2(obj_vel.x * std::sin(yaw), obj_vel.x * std::cos(yaw)); - return tier4_autoware_utils::normalizeRadian( + return autoware_universe_utils::normalizeRadian( pedestrian_heading_direction - pedestrian_to_crosswalk_center_direction); }(); @@ -658,7 +658,7 @@ ObjectClassification::_label_type changeLabelForPrediction( if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; constexpr float high_speed_threshold = - tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + autoware_universe_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -673,7 +673,7 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::PEDESTRIAN: { const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float max_velocity_for_human_mps = - tier4_autoware_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + autoware_universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); @@ -859,13 +859,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); processing_time_publisher_ = - std::make_unique(this, "map_based_prediction"); + std::make_unique(this, "map_based_prediction"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); - stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -873,7 +875,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "max_lateral_accel", max_lateral_accel_); updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); @@ -1008,14 +1010,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt if ( label_for_prediction == ObjectClassification::PEDESTRIAN || label_for_prediction == ObjectClassification::BICYCLE) { - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware_universe_utils::toHexString(object.object_id); current_crosswalk_users.emplace(object_id, object); } } std::unordered_set predicted_crosswalk_users_ids; for (const auto & object : in_objects->objects) { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware_universe_utils::toHexString(object.object_id); TrackedObject transformed_object = object; // transform object frame if it's based on map frame @@ -1034,7 +1036,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt switch (label) { case ObjectClassification::PEDESTRIAN: case ObjectClassification::BICYCLE: { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware_universe_utils::toHexString(object.object_id); if (match_lost_and_appeared_crosswalk_users_) { object_id = tryMatchNewObjectToDisappeared(object_id, current_crosswalk_users); } @@ -1337,11 +1339,11 @@ bool MapBasedPredictionNode::isIntersecting( const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) { - const auto p1 = tier4_autoware_utils::createPoint(point1.x, point1.y, 0.0); - const auto p2 = tier4_autoware_utils::createPoint(point2.x, point2.y, 0.0); - const auto p3 = tier4_autoware_utils::createPoint(point3.x(), point3.y(), 0.0); - const auto p4 = tier4_autoware_utils::createPoint(point4.x(), point4.y(), 0.0); - const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + const auto p1 = autoware_universe_utils::createPoint(point1.x, point1.y, 0.0); + const auto p2 = autoware_universe_utils::createPoint(point2.x, point2.y, 0.0); + const auto p3 = autoware_universe_utils::createPoint(point3.x(), point3.y(), 0.0); + const auto p4 = autoware_universe_utils::createPoint(point4.x(), point4.y(), 0.0); + const auto intersection = autoware_universe_utils::intersect(p1, p2, p3, p4); return intersection.has_value(); } @@ -1489,7 +1491,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) // Compute yaw angle from the velocity and position of the object const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const auto future_object_pose = tier4_autoware_utils::calcOffsetPose( + const auto future_object_pose = autoware_universe_utils::calcOffsetPose( object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); // assumption: the object vx is much larger than vy @@ -1508,15 +1510,16 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle object.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw); + autoware_universe_utils::createQuaternionFromYaw( + autoware_universe_utils::pi + original_yaw); break; } default: { - const auto updated_object_yaw = - tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); + const auto updated_object_yaw = autoware_universe_utils::calcAzimuthAngle( + object_pose.position, future_object_pose.position); object.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw); + autoware_universe_utils::createQuaternionFromYaw(updated_object_yaw); break; } } @@ -1533,7 +1536,7 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( const bool isDisappeared = std::none_of( in_objects->objects.begin(), in_objects->objects.end(), [&it](autoware_perception_msgs::msg::TrackedObject obj) { - return tier4_autoware_utils::toHexString(obj.object_id) == it->first.first; + return autoware_universe_utils::toHexString(obj.object_id) == it->first.first; }); if (isDisappeared) { it = stopped_times_against_green_.erase(it); @@ -1642,7 +1645,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware_universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) != 0) { const std::vector & possible_lanelet = road_users_history.at(object_id).back().future_possible_lanelets; @@ -1660,7 +1663,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet.second, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); // Step3. Check if the closest lanelet is valid, and add all @@ -1687,7 +1690,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); const double delta_yaw = obj_yaw - lane_yaw; - const double abs_norm_delta_yaw = std::fabs(tier4_autoware_utils::normalizeRadian(delta_yaw)); + const double abs_norm_delta_yaw = std::fabs(autoware_universe_utils::normalizeRadian(delta_yaw)); // compute lateral distance const auto centerline = current_lanelet.centerline(); @@ -1716,7 +1719,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware_universe_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); ObjectData single_object_data; @@ -1725,7 +1728,8 @@ void MapBasedPredictionNode::updateRoadUsersHistory( single_object_data.future_possible_lanelets = current_lanelets; single_object_data.pose = object.kinematics.pose_with_covariance.pose; const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - single_object_data.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(object_yaw); + single_object_data.pose.orientation = + autoware_universe_utils::createQuaternionFromYaw(object_yaw); single_object_data.time_delay = std::fabs((this->get_clock()->now() - header.stamp).seconds()); single_object_data.twist = object.kinematics.twist_with_covariance.twist; @@ -1929,7 +1933,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( throw std::logic_error("Lane change detection method is invalid."); }(); - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware_universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return current_maneuver; } @@ -1968,7 +1972,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( const double /*object_detected_time*/) { // Step1. Check if we have the object in the buffer - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware_universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -2039,7 +2043,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( const double /*object_detected_time*/) { // Step1. Check if we have the object in the buffer - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware_universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -2077,7 +2081,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( for (const auto & lanelet : prev_lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.position); const double delta_yaw = tf2::getYaw(prev_pose.orientation) - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); if (normalized_delta_yaw < closest_prev_yaw) { closest_prev_yaw = normalized_delta_yaw; prev_lanelet = lanelet; @@ -2087,7 +2091,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( // Step4. Check if the vehicle has changed lane const auto current_lanelet = current_lanelet_data.lanelet; const auto current_pose = object.kinematics.pose_with_covariance.pose; - const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose); + const double dist = autoware_universe_utils::calcDistance2d(prev_pose, current_pose); lanelet::routing::LaneletPaths possible_paths = routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); bool has_lane_changed = true; @@ -2172,8 +2176,8 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( const double curr_yaw = prev_yaw + wz * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, curr_z); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + current_pose.position = autoware_universe_utils::createPoint(curr_x, curr_y, curr_z); + current_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(curr_yaw); return current_pose; } @@ -2185,7 +2189,7 @@ double MapBasedPredictionNode::calcRightLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware_universe_utils::createPoint(x, y, 0.0); } return std::fabs(motion_utils::calcLateralOffset(boundary_path, search_pose.position)); @@ -2200,7 +2204,7 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware_universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return; } @@ -2327,7 +2331,7 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + current_p.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } @@ -2349,7 +2353,7 @@ std::vector MapBasedPredictionNode::convertPathType( // Prevent from inserting same points if (!converted_path.empty()) { const auto last_p = converted_path.back(); - const double tmp_dist = tier4_autoware_utils::calcDistance2d(last_p, current_p); + const double tmp_dist = autoware_universe_utils::calcDistance2d(last_p, current_p); if (tmp_dist < 1e-6) { prev_p = current_p; continue; @@ -2358,7 +2362,7 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + current_p.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } @@ -2398,7 +2402,7 @@ bool MapBasedPredictionNode::isDuplicated( for (const auto & prev_predicted_path : predicted_paths) { const auto prev_path_end = prev_predicted_path.path.back().position; const auto current_path_end = predicted_path.path.back().position; - const double dist = tier4_autoware_utils::calcDistance2d(prev_path_end, current_path_end); + const double dist = autoware_universe_utils::calcDistance2d(prev_path_end, current_path_end); if (dist < CLOSE_PATH_THRESHOLD) { return true; } @@ -2447,10 +2451,11 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN; }(); - const auto key = std::make_pair(tier4_autoware_utils::toHexString(object.object_id), signal_id); + const auto key = + std::make_pair(autoware_universe_utils::toHexString(object.object_id), signal_id); if ( signal_color == TrafficLightElement::GREEN && - tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + autoware_universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < threshold_velocity_assumed_as_stopping_) { stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 873219ffcf130..2f811e89f5296 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -14,9 +14,9 @@ #include "map_based_prediction/path_generator.hpp" +#include #include #include -#include #include @@ -49,8 +49,9 @@ PredictedPath PathGenerator::generatePathToTargetPoint( const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); - const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( - std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); + const auto pedestrian_to_entry_point_orientation = + autoware_universe_utils::createQuaternionFromYaw(std::atan2( + pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); for (double dt = 0.0; dt < arrival_time + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; @@ -92,10 +93,11 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); - const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( - std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); + const auto pedestrian_to_entry_point_orientation = + autoware_universe_utils::createQuaternionFromYaw(std::atan2( + pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); const auto entry_to_exit_point_normalized = entry_to_exit_point.normalized(); - const auto entry_to_exit_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( + const auto entry_to_exit_point_orientation = autoware_universe_utils::createQuaternionFromYaw( std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x())); for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { @@ -172,7 +174,7 @@ PredictedPath PathGenerator::generateStraightPath( path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); path.path.reserve(static_cast((duration) / sampling_time_interval_)); for (double dt = 0.0; dt < duration; dt += sampling_time_interval_) { - const auto future_obj_pose = tier4_autoware_utils::calcOffsetPose( + const auto future_obj_pose = autoware_universe_utils::calcOffsetPose( object_pose, object_twist.linear.x * dt, object_twist.linear.y * dt, 0.0); path.path.push_back(future_obj_pose); } @@ -317,7 +319,7 @@ PosePath PathGenerator::interpolateReferencePath( base_path_y.at(i) = base_path.at(i).position.y; base_path_z.at(i) = base_path.at(i).position.z; if (i > 0) { - base_path_s.at(i) = base_path_s.at(i - 1) + tier4_autoware_utils::calcDistance2d( + base_path_s.at(i) = base_path_s.at(i - 1) + autoware_universe_utils::calcDistance2d( base_path.at(i - 1), base_path.at(i)); } } @@ -342,16 +344,16 @@ PosePath PathGenerator::interpolateReferencePath( for (size_t i = 0; i < interpolate_num - 1; ++i) { geometry_msgs::msg::Pose interpolated_pose; const auto current_point = - tier4_autoware_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); - const auto next_point = tier4_autoware_utils::createPoint( + autoware_universe_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); + const auto next_point = autoware_universe_utils::createPoint( spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point); - interpolated_pose.position = tier4_autoware_utils::createPoint( + const double yaw = autoware_universe_utils::calcAzimuthAngle(current_point, next_point); + interpolated_pose.position = autoware_universe_utils::createPoint( spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); - interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + interpolated_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); interpolated_path.at(i) = interpolated_pose; } - interpolated_path.back().position = tier4_autoware_utils::createPoint( + interpolated_path.back().position = autoware_universe_utils::createPoint( spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; @@ -373,14 +375,15 @@ PredictedPath PathGenerator::convertToPredictedPath( const auto & frenet_point = frenet_predicted_path.at(i); // Converted Pose - auto predicted_pose = tier4_autoware_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); + auto predicted_pose = + autoware_universe_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); predicted_pose.position.z = object.kinematics.pose_with_covariance.pose.position.z; if (i == 0) { predicted_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; } else { - const double yaw = tier4_autoware_utils::calcAzimuthAngle( + const double yaw = autoware_universe_utils::calcAzimuthAngle( predicted_path.path.at(i - 1).position, predicted_pose.position); - predicted_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + predicted_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); } predicted_path.path.at(i) = predicted_pose; } diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp index f36116378af78..adc02680e0d94 100644 --- a/perception/cluster_merger/include/cluster_merger/node.hpp +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -15,11 +15,11 @@ #ifndef CLUSTER_MERGER__NODE_HPP_ #define CLUSTER_MERGER__NODE_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "message_filters/subscriber.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" @@ -59,7 +59,7 @@ class ClusterMergerNode : public rclcpp::Node std::string output_frame_id_; std::vector::SharedPtr> sub_objects_array{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; void objectsCallback( const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml index 220837bfd7e4e..17455dc27b6cc 100644 --- a/perception/cluster_merger/package.xml +++ b/perception/cluster_merger/package.xml @@ -12,12 +12,12 @@ ament_cmake_auto + autoware_universe_utils geometry_msgs message_filters object_recognition_utils rclcpp rclcpp_components - tier4_autoware_utils tier4_perception_msgs autoware_cmake ament_lint_auto diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index d56cb3fb31584..a76b0fc56e2a0 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -44,7 +44,7 @@ ament_target_dependencies(compare_map_segmentation rclcpp rclcpp_components sensor_msgs - tier4_autoware_utils + autoware_universe_utils autoware_map_msgs nav_msgs ) diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index ca427db9811ba..7403dc0713475 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_map_msgs + autoware_universe_utils grid_map_pcl grid_map_ros nav_msgs @@ -26,7 +27,6 @@ rclcpp rclcpp_components sensor_msgs - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index 0f870d37c0de1..f72e87e2e3610 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -109,8 +109,8 @@ DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distance_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 7ad479a6e74bd..6e8f527cc465c 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -72,8 +72,8 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_approximate_compare_map_filter"); diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 595336145dabd..720b5cf1d229a 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -31,8 +31,8 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index f169e2069aee7..3aacf31c9332f 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -101,8 +101,8 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_distance_based_compare_map_filter"); diff --git a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp index 424ff87cbc0d7..3a29e7dd1d533 100644 --- a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp +++ b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp @@ -15,8 +15,8 @@ #ifndef DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ #define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ +#include #include -#include #include #include @@ -36,7 +36,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input); void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs); }; diff --git a/perception/detected_object_feature_remover/package.xml b/perception/detected_object_feature_remover/package.xml index 4beafa18d5146..9eb47e0e69557 100644 --- a/perception/detected_object_feature_remover/package.xml +++ b/perception/detected_object_feature_remover/package.xml @@ -11,9 +11,9 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils rclcpp rclcpp_components - tier4_autoware_utils tier4_perception_msgs ament_cmake_ros diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp index c5f977a4a3354..43a1d4d3d26ea 100644 --- a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp @@ -23,7 +23,8 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt pub_ = this->create_publisher("~/output", rclcpp::QoS(1)); sub_ = this->create_subscription( "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1)); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void DetectedObjectFeatureRemover::objectCallback( diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index fa28362d85fe8..b96a57b12a233 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -17,11 +17,11 @@ #include "detected_object_validation/utils/utils.hpp" +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -35,10 +35,10 @@ namespace object_lanelet_filter { -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::MultiPoint2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; class ObjectLaneletFilterNode : public rclcpp::Node { @@ -52,7 +52,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; - std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; @@ -89,7 +89,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node const autoware_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_lanelet_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index 02ec00fe0457f..d9eb3d367d4cf 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -17,9 +17,9 @@ #include "detected_object_validation/utils/utils.hpp" +#include +#include #include -#include -#include #include #include @@ -54,7 +54,7 @@ class ObjectPositionFilterNode : public rclcpp::Node utils::FilterTargetLabel filter_target_; bool isObjectInBounds(const autoware_perception_msgs::msg::DetectedObject & object) const; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_position_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 5362dab62432a..450072f258e2a 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -19,9 +19,9 @@ #include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" +#include +#include #include -#include -#include #include #include @@ -131,7 +131,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr objects_pub_; message_filters::Subscriber objects_sub_; message_filters::Subscriber obstacle_pointcloud_sub_; - std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr debug_publisher_{nullptr}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -145,7 +145,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; std::unique_ptr validator_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; private: void onObjectsAndObstaclePointCloud( diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index 62c3e43d8e730..399db8cd8c1f3 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -15,11 +15,11 @@ #ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ #define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#include #include #include #include #include -#include #include #include @@ -43,7 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node message_filters::Subscriber occ_grid_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; typedef message_filters::sync_policies::ApproximateTime< autoware_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid> diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index c48785438324c..7abfbe6625f01 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -19,6 +19,7 @@ autoware_map_msgs autoware_perception_msgs + autoware_universe_utils geometry_msgs lanelet2_extension message_filters @@ -30,7 +31,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 05c5426944ec7..42247a2e36d2b 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -14,10 +14,10 @@ #include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" +#include #include #include #include -#include #include #include @@ -63,8 +63,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "output/object", rclcpp::QoS{1}); debug_publisher_ = - std::make_unique(this, "object_lanelet_filter"); - published_time_publisher_ = std::make_unique(this); + std::make_unique(this, "object_lanelet_filter"); + published_time_publisher_ = + std::make_unique(this); } void ObjectLaneletFilterNode::mapCallback( @@ -236,7 +237,7 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( const auto footprint = setFootprint(object); for (const auto & point : footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + autoware_universe_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); @@ -245,7 +246,7 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( // if object do not have bounding box, check each footprint is inside polygon for (const auto & point : object.shape.footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + autoware_universe_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); geometry_msgs::msg::Pose point2d; point2d.position.x = point_transformed.x; point2d.position.y = point_transformed.y; @@ -295,7 +296,7 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_velocity_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 278b4829cae40..8e22cd1b88d88 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -42,7 +42,8 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void ObjectPositionFilterNode::objectCallback( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 0dd3c1ed08f15..10e923b91c1bd 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -14,8 +14,8 @@ #include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#include #include -#include #include @@ -33,7 +33,7 @@ namespace obstacle_pointcloud_based_validator { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Polygon2d = autoware_universe_utils::Polygon2d; Validator::Validator(PointsNumThresholdParam & points_num_threshold_param) { @@ -97,7 +97,7 @@ std::optional Validator2D::getPointCloudWithinObject( std::vector vertices_array; pcl::Vertices vertices; Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + autoware_universe_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -212,7 +212,7 @@ std::optional Validator3D::getPointCloudWithinObject( auto z_min = object_position.z - object_height / 2.0f; auto z_max = object_position.z + object_height / 2.0f; Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + autoware_universe_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -304,12 +304,13 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - debug_publisher_ = std::make_unique( + debug_publisher_ = std::make_unique( this, "obstacle_pointcloud_based_validator"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 792483eeebf81..ec29818ee6491 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -14,9 +14,9 @@ #include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#include #include #include -#include #include @@ -33,7 +33,7 @@ namespace occupancy_grid_based_validator { using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Polygon2d = autoware_universe_utils::Polygon2d; OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options) : rclcpp::Node("occupancy_grid_based_validator", node_options), @@ -52,7 +52,8 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -107,7 +108,7 @@ std::optional OccupancyGridBasedValidator::getMask( const auto & origin = occupancy_grid.info.origin; std::vector pixel_vertices; Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + autoware_universe_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); bool is_polygon_within_image = true; for (const auto & p : poly2d.outer()) { diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index 51e93bfea33dd..40caa06d3acbf 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -15,13 +15,13 @@ #ifndef DETECTION_BY_TRACKER__DEBUGGER_HPP_ #define DETECTION_BY_TRACKER__DEBUGGER_HPP_ +#include +#include #include #include #include #include #include -#include -#include #include #include @@ -60,9 +60,9 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); processing_time_publisher_ = - std::make_unique(node, "detection_by_tracker"); + std::make_unique(node, "detection_by_tracker"); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); this->startStopWatch(); } @@ -103,8 +103,8 @@ class Debugger rclcpp::Publisher::SharedPtr merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; autoware_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index d0e5cc8436ec8..de9453c68ca95 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -18,12 +18,12 @@ #include "detection_by_tracker/debugger.hpp" #include "detection_by_tracker/utils.hpp" +#include #include #include #include #include #include -#include #include #include @@ -84,7 +84,7 @@ class DetectionByTracker : public rclcpp::Node detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml index dae58f2d7da78..52d4d3fcdd7a2 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/detection_by_tracker/package.xml @@ -13,6 +13,7 @@ autoware_cmake eigen3_cmake_module + autoware_universe_utils eigen euclidean_cluster libpcl-all-dev @@ -24,7 +25,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 8ee6cc2739b43..dd63013bff038 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -16,8 +16,8 @@ #include "object_recognition_utils/object_recognition_utils.hpp" -#include -#include +#include +#include #include #include @@ -61,7 +61,7 @@ boost::optional getReferenceYawInfo(const uint8_t label, const const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceYawInfo{yaw, tier4_autoware_utils::deg2rad(30)}; + return ReferenceYawInfo{yaw, autoware_universe_utils::deg2rad(30)}; } else { return boost::none; } @@ -136,9 +136,9 @@ bool TrackerHandler::estimateTrackedObjects( estimated_object.kinematics.pose_with_covariance.pose.position.y = y + vx * std::sin(yaw) * dt.seconds(); estimated_object.kinematics.pose_with_covariance.pose.position.z = z; - const float yaw_hat = tier4_autoware_utils::normalizeRadian(yaw + wz * dt.seconds()); + const float yaw_hat = autoware_universe_utils::normalizeRadian(yaw + wz * dt.seconds()); estimated_object.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_hat); + autoware_universe_utils::createQuaternionFromYaw(yaw_hat); output.objects.push_back(estimated_object); } return true; @@ -177,7 +177,8 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void DetectionByTracker::setMaxSearchRange() @@ -283,7 +284,7 @@ void DetectionByTracker::divideUnderSegmentedObjects( for (const auto & initial_object : in_cluster_objects.feature_objects) { // search near object - const float distance = tier4_autoware_utils::calcDistance2d( + const float distance = autoware_universe_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); if (max_search_range < distance) { @@ -419,7 +420,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( pcl::PointCloud pcl_merged_cluster; for (const auto & initial_object : in_cluster_objects.feature_objects) { - const float distance = tier4_autoware_utils::calcDistance2d( + const float distance = autoware_universe_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 88f7bf4a9c2a9..fda0fcddc1bc7 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -15,12 +15,12 @@ #ifndef ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ #define ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ +#include #include #include #include #include #include -#include #include "tier4_external_api_msgs/msg/map_hash.hpp" #include diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index e287b83aaa504..afbd88da84bde 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -14,6 +14,7 @@ autoware_grid_map_utils autoware_map_msgs + autoware_universe_utils grid_map_cv grid_map_pcl grid_map_ros @@ -25,7 +26,6 @@ rosbag2_storage_default_plugins tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_external_api_msgs ament_lint_auto diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 7b7c529e17dcc..1d3d091165b4c 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -356,7 +356,7 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) // Convert elevation layer to OpenCV image to fill in holes. // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). namespace bg = boost::geometry; - using tier4_autoware_utils::Point2d; + using autoware_universe_utils::Point2d; elevation_map_.add("inpaint_mask", 0.0); diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index 746ef1bafb583..31f24766de9a9 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -38,9 +38,10 @@ EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); - stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_ = + std::make_unique>(); debug_publisher_ = - std::make_unique(this, "euclidean_cluster"); + std::make_unique(this, "euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 16e179b2512d3..23724ae880827 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -16,9 +16,9 @@ #include "euclidean_cluster/euclidean_cluster.hpp" +#include +#include #include -#include -#include #include #include @@ -41,8 +41,8 @@ class EuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 7e6a456561900..2160fd50faafb 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -42,8 +42,9 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique( + stop_watch_ptr_ = + std::make_unique>(); + debug_publisher_ = std::make_unique( this, "voxel_grid_based_euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index 18330b5f12074..3a954436ce8ca 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -16,9 +16,9 @@ #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include +#include #include -#include -#include #include #include @@ -41,8 +41,8 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster 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 314c9cfb97a75..39498f9c8c82f 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 @@ -280,9 +280,9 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult onParameter(const std::vector & p); // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index e638baba12521..7af02da734524 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 #include #include @@ -26,11 +26,11 @@ namespace ground_segmentation { using autoware::vehicle_info_utils::VehicleInfoUtils; +using autoware_universe_utils::calcDistance3d; +using autoware_universe_utils::deg2rad; +using autoware_universe_utils::normalizeDegree; +using autoware_universe_utils::normalizeRadian; 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; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("ScanGroundFilter", options) @@ -79,8 +79,8 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "scan_ground_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index a2a78c99cd5ba..92859a80e48f6 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -15,10 +15,10 @@ #ifndef IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ #define IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#include +#include #include #include -#include -#include #include #include @@ -138,8 +138,8 @@ class FusionNode : public rclcpp::Node float filter_scope_max_z_; /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index a59b6992c5328..65e03c6bc6316 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -15,8 +15,8 @@ #ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ #define IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ +#include "autoware/universe_utils/ros/debug_publisher.hpp" #include "image_projection_based_fusion/fusion_node.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "autoware_perception_msgs/msg/object_classification.hpp" diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 0b6497135612e..ffa1666396a1d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -34,10 +34,10 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include #include #include #include -#include #include "autoware_perception_msgs/msg/shape.hpp" #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index c1bd85cb945de..8d5a2ef1fe519 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -18,6 +18,7 @@ autoware_perception_msgs autoware_point_types + autoware_universe_utils cv_bridge euclidean_cluster image_transport @@ -33,7 +34,6 @@ tf2_eigen tf2_ros tf2_sensor_msgs - tier4_autoware_utils tier4_perception_msgs ament_cmake_gtest diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index fa2dcd2b0e7e1..e561642b76951 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -130,8 +130,8 @@ FusionNode::FusionNode( // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 24ff1245813ac..7973c22cc1d78 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -16,6 +16,8 @@ #include "autoware_point_types/types.hpp" +#include +#include #include #include #include @@ -23,8 +25,6 @@ #include #include #include -#include -#include #include diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index d44620995c61b..c1e431ed83c99 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -14,10 +14,10 @@ #include "image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include #include #include #include -#include #include #include diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 950c73bc2fb6d..fb99364147509 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -70,7 +70,7 @@ void closest_cluster( std::memcpy(&point.y, &cluster.data[i * point_step + y_offset], sizeof(float)); std::memcpy(&point.z, &cluster.data[i * point_step + z_offset], sizeof(float)); - point_data.distance = tier4_autoware_utils::calcDistance2d(center, point); + point_data.distance = autoware_universe_utils::calcDistance2d(center, point); point_data.orig_index = i; points_data.push_back(point_data); } @@ -258,7 +258,7 @@ pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); for (std::size_t i = 0; i < cluster.points.size(); ++i) { pcl::PointXYZ point = cluster.points.at(i); - double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + double dist_closest_point = autoware_universe_utils::calcDistance2d(point, orig_point); if (min_dist > dist_closest_point) { min_dist = dist_closest_point; closest_point = pcl::PointXYZ(point.x, point.y, point.z); diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index c4c73927ad255..462e9d22f9da2 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(tier4_autoware_utils REQUIRED) +find_package(autoware_universe_utils REQUIRED) find_package(tier4_debug_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) @@ -40,7 +40,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE "$" ${cuda_utils_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} - ${tier4_autoware_utils_INCLUDE_DIRS} + ${autoware_universe_utils_INCLUDE_DIRS} ${autoware_perception_msgs_INCLUDE_DIRS} ) diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp index ca2ae95149e9a..1e5966f5751af 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp @@ -19,11 +19,11 @@ #include "feature_generator.hpp" #include "lidar_apollo_instance_segmentation/node.hpp" +#include #include #include #include #include -#include #include #include diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp index 0ed5f37b6fa14..9082e209fb330 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp @@ -17,9 +17,9 @@ #include "lidar_apollo_instance_segmentation/debugger.hpp" +#include +#include #include -#include -#include #include #include @@ -47,8 +47,8 @@ class LidarInstanceSegmentationNode : public rclcpp::Node std::shared_ptr detector_ptr_; std::shared_ptr debugger_ptr_; void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; public: explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/lidar_apollo_instance_segmentation/package.xml index c5b3dbb9c0585..1157b7575933c 100755 --- a/perception/lidar_apollo_instance_segmentation/package.xml +++ b/perception/lidar_apollo_instance_segmentation/package.xml @@ -13,6 +13,7 @@ ament_cmake autoware_perception_msgs + autoware_universe_utils cuda_utils libpcl-all-dev pcl_conversions @@ -20,7 +21,6 @@ rclcpp_components tensorrt_common tf2_eigen - tier4_autoware_utils tier4_debug_msgs tier4_perception_msgs diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index 236f0998ab21f..592c63ed22169 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -88,7 +88,7 @@ bool LidarApolloInstanceSegmentation::transformCloud( target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500)); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - tier4_autoware_utils::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix); + autoware_universe_utils::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix); transformed_cloud.header.frame_id = target_frame_; pcl_transformed_cloud.header.frame_id = target_frame_; } catch (tf2::TransformException & ex) { @@ -103,7 +103,7 @@ bool LidarApolloInstanceSegmentation::transformCloud( pcl::PointCloud pointcloud_with_z_offset; Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform); pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud); diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/lidar_apollo_instance_segmentation/src/node.cpp index 5bf8ed0d027e5..6034fe917bf42 100644 --- a/perception/lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/node.cpp @@ -16,8 +16,8 @@ #include "lidar_apollo_instance_segmentation/detector.hpp" -#include -#include +#include +#include namespace lidar_apollo_instance_segmentation { @@ -28,8 +28,8 @@ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( using std::placeholders::_1; // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "lidar_apollo_instance_segmentation"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml index 54b4e5d4b7836..643a331e54a5a 100755 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ b/perception/lidar_apollo_segmentation_tvm/package.xml @@ -14,6 +14,7 @@ libpcl-all-dev autoware_perception_msgs + autoware_universe_utils geometry_msgs pcl_conversions rclcpp @@ -21,7 +22,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs tvm_utility tvm_vendor diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 85f0a69356d55..a516d35edfd6b 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include -#include #include #include @@ -147,7 +147,7 @@ void ApolloLidarSegmentation::transformCloud( tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( in_cluster, transformed_cloud_cluster, affine_matrix); transformed_cloud_cluster.header.frame_id = target_frame_; } else { @@ -158,7 +158,7 @@ void ApolloLidarSegmentation::transformCloud( if (z_offset != 0) { Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform); } diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index fa9c84c41b394..d96eb1ae3ed94 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -17,12 +17,12 @@ #include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include +#include +#include #include #include #include -#include -#include -#include #include #include @@ -62,11 +62,11 @@ class LidarCenterPointNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 2804e172b73fa..f7b0fe5e0c58f 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -14,10 +14,10 @@ #include "lidar_centerpoint/centerpoint_trt.hpp" +#include #include #include #include -#include #include #include diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index d4d7b1379776a..81f620018cf04 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,9 @@ #include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" namespace centerpoint { @@ -48,7 +48,7 @@ bool NonMaximumSuppression::isTargetPairObject( } const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; - const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware_universe_utils::calcSquaredDistance2d( object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 9be77b67e26dc..4799ec322d779 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "lidar_centerpoint/ros_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" namespace centerpoint { @@ -50,14 +50,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - tier4_autoware_utils::pi / 2; + float yaw = -box3d.yaw - autoware_universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware_universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware_universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware_universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); if (has_variance) { obj.kinematics.has_position_covariance = has_variance; obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); @@ -102,7 +102,7 @@ uint8_t getSemanticType(const std::string & class_name) std::array convertPoseCovarianceMatrix(const Box3D & box3d) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = box3d.x_variance; pose_covariance[POSE_IDX::Y_Y] = box3d.y_variance; @@ -113,7 +113,7 @@ std::array convertPoseCovarianceMatrix(const Box3D & box3d) std::array convertTwistCovarianceMatrix(const Box3D & box3d) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array twist_covariance{}; twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 3077aa2ac2e24..280e2219fb1f1 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils object_recognition_utils pcl_ros python3-open3d @@ -22,7 +23,6 @@ tf2_eigen tf2_geometry_msgs tf2_sensor_msgs - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index b9415ba898d14..b65c70f02bd1b 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -117,8 +117,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint"); stop_watch_ptr_->tic("cyclic_time"); @@ -129,7 +129,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void LidarCenterPointNode::pointCloudCallback( diff --git a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp index 7a7d3ff3f1f15..cb387c404c07e 100644 --- a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp +++ b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp @@ -15,11 +15,11 @@ #ifndef LIDAR_CENTERPOINT_TVM__NODE_HPP_ #define LIDAR_CENTERPOINT_TVM__NODE_HPP_ +#include +#include #include #include #include -#include -#include #include #include @@ -59,9 +59,9 @@ class LIDAR_CENTERPOINT_TVM_PUBLIC LidarCenterPointTVMNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace lidar_centerpoint_tvm diff --git a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp index 2e0f9ad28bfb6..91f727448b76a 100644 --- a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp +++ b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp @@ -14,12 +14,12 @@ #include "lidar_centerpoint_tvm/centerpoint_tvm.hpp" +#include #include #include #include #include #include -#include #include #include diff --git a/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp b/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp index cde4b0fe4112d..16d7094e1e628 100644 --- a/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp @@ -14,8 +14,8 @@ #include "lidar_centerpoint_tvm/ros_utils.hpp" -#include -#include +#include +#include namespace autoware { @@ -65,14 +65,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - tier4_autoware_utils::pi / 2; + float yaw = -box3d.yaw - autoware_universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware_universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware_universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware_universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); // twist if (has_twist) { diff --git a/perception/lidar_centerpoint_tvm/package.xml b/perception/lidar_centerpoint_tvm/package.xml index eac9bc3d2bf45..c71a27e4a1677 100644 --- a/perception/lidar_centerpoint_tvm/package.xml +++ b/perception/lidar_centerpoint_tvm/package.xml @@ -12,13 +12,13 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils pcl_ros rclcpp rclcpp_components tf2_eigen tf2_geometry_msgs tf2_sensor_msgs - tier4_autoware_utils tvm_utility tvm_vendor diff --git a/perception/lidar_centerpoint_tvm/src/node.cpp b/perception/lidar_centerpoint_tvm/src/node.cpp index 288b8d59aec21..fec19a479a89f 100644 --- a/perception/lidar_centerpoint_tvm/src/node.cpp +++ b/perception/lidar_centerpoint_tvm/src/node.cpp @@ -94,8 +94,8 @@ LidarCenterPointTVMNode::LidarCenterPointTVMNode(const rclcpp::NodeOptions & nod // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint_tvm"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp index 86303618d7877..d57405b1c3b7b 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp @@ -21,10 +21,10 @@ #include "lidar_transfusion/transfusion_trt.hpp" #include "lidar_transfusion/visibility_control.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -63,10 +63,10 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_pub_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr published_time_pub_{nullptr}; }; } // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp index a94c3c1871136..d454932935b64 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp @@ -24,7 +24,7 @@ #include "lidar_transfusion/utils.hpp" #include "lidar_transfusion/visibility_control.hpp" -#include +#include #include @@ -83,7 +83,7 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT std::unique_ptr network_trt_ptr_{nullptr}; std::unique_ptr vg_ptr_{nullptr}; - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; std::unique_ptr pre_ptr_{nullptr}; std::unique_ptr post_ptr_{nullptr}; diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index c1179a3f6673d..a8b4ddbfd5db9 100644 --- a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,9 @@ #include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include #include #include -#include namespace lidar_transfusion { @@ -49,7 +49,7 @@ bool NonMaximumSuppression::isTargetPairObject( } const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; - const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware_universe_utils::calcSquaredDistance2d( object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/lidar_transfusion/lib/ros_utils.cpp index 0ecb34faf732d..cdfa693af2219 100644 --- a/perception/lidar_transfusion/lib/ros_utils.cpp +++ b/perception/lidar_transfusion/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "lidar_transfusion/ros_utils.hpp" +#include +#include #include -#include -#include namespace lidar_transfusion { @@ -49,14 +49,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = box3d.yaw + tier4_autoware_utils::pi / 2; + float yaw = box3d.yaw + autoware_universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware_universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware_universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware_universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); } uint8_t getSemanticType(const std::string & class_name) diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/lidar_transfusion/lib/transfusion_trt.cpp index c876c0906162b..4a8416a16e18c 100644 --- a/perception/lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/lidar_transfusion/lib/transfusion_trt.cpp @@ -17,7 +17,7 @@ #include "lidar_transfusion/preprocess/preprocess_kernel.hpp" #include "lidar_transfusion/transfusion_config.hpp" -#include +#include #include #include @@ -37,7 +37,8 @@ TransfusionTRT::TransfusionTRT( network_trt_ptr_->init( network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); vg_ptr_ = std::make_unique(densification_param, config_, stream_); - stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("processing/inner"); initPtr(); diff --git a/perception/lidar_transfusion/package.xml b/perception/lidar_transfusion/package.xml index 29643d0957eb1..ffbe091b998e5 100644 --- a/perception/lidar_transfusion/package.xml +++ b/perception/lidar_transfusion/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils launch_ros object_recognition_utils pcl_ros @@ -22,7 +23,6 @@ tf2_eigen tf2_geometry_msgs tf2_sensor_msgs - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp index 9313015002973..07b19f1db51f8 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -94,12 +94,12 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) objects_pub_ = this->create_publisher( "~/output/objects", rclcpp::QoS(1)); - published_time_pub_ = std::make_unique(this); + published_time_pub_ = std::make_unique(this); // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic"); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index c685024d6ef8b..7c1b814258bcf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -17,8 +17,8 @@ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include #include -#include #include "unique_identifier_msgs/msg/uuid.hpp" #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 9ab5576faccd8..08d36c80d4cf8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -17,11 +17,11 @@ #include "multi_object_tracker/debugger/debug_object.hpp" +#include +#include #include #include #include -#include -#include #include #include @@ -56,7 +56,7 @@ class TrackerDebugger rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr processing_time_publisher_; rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; diagnostic_updater::Updater diagnostic_updater_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 569bc4f43a3d4..b24eadd586e73 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -73,7 +73,7 @@ class MultiObjectTracker : public rclcpp::Node // debugger std::unique_ptr debugger_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 4e2696700bfdc..67f56273e79c5 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -14,6 +14,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils diagnostic_updater eigen glog @@ -24,7 +25,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils unique_identifier_msgs ament_lint_auto diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 0ff3188e069f2..d033e98cce46e 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -52,8 +52,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { const double measurement_yaw = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + autoware_universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware_universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -170,7 +170,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware_universe_utils::calcDistance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -183,7 +183,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = tier4_autoware_utils::getArea(measurement_object.shape); + const double area = autoware_universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 7b4f9d717a927..bb5fdef9be690 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -24,7 +24,7 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ // initialize debug publishers if (debug_settings_.publish_processing_time) { processing_time_publisher_ = - std::make_unique(&node_, "multi_object_tracker"); + std::make_unique(&node_, "multi_object_tracker"); } if (debug_settings_.publish_tentative_objects) { diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 70433f16125c9..af61cfb8259b1 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -199,7 +199,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Debugger debugger_ = std::make_unique(*this, world_frame_id_); debugger_->setObjectChannels(input_names_short); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void MultiObjectTracker::onTrigger() diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index b54c9b832b0cf..fa7adb8396612 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -57,9 +57,9 @@ BicycleTracker::BicycleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - double r_stddev_x = 0.5; // in vehicle coordinate [m] - double r_stddev_y = 0.4; // in vehicle coordinate [m] - double r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad] + double r_stddev_x = 0.5; // in vehicle coordinate [m] + double r_stddev_y = 0.4; // in vehicle coordinate [m] + double r_stddev_yaw = autoware_universe_utils::deg2rad(30); // in map coordinate [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -102,8 +102,8 @@ BicycleTracker::BicycleTracker( // Set motion limits { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware_universe_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -126,7 +126,7 @@ BicycleTracker::BicycleTracker( constexpr double p0_stddev_x = 0.8; // in object coordinate [m] constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + autoware_universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -145,14 +145,15 @@ BicycleTracker::BicycleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware_universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double p0_stddev_slip = + autoware_universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -327,7 +328,7 @@ bool BicycleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index ebf5a977ed44a..92815f5e33a6b 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -58,10 +58,10 @@ BigVehicleTracker::BigVehicleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = autoware_universe_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -70,7 +70,7 @@ BigVehicleTracker::BigVehicleTracker( // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + velocity_deviation_threshold_ = autoware_universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -120,8 +120,8 @@ BigVehicleTracker::BigVehicleTracker( // Set motion limits { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware_universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -144,7 +144,7 @@ BigVehicleTracker::BigVehicleTracker( constexpr double p0_stddev_x = 1.5; // in object coordinate [m] constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + autoware_universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -163,14 +163,15 @@ BigVehicleTracker::BigVehicleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware_universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double p0_stddev_slip = + autoware_universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -422,7 +423,7 @@ bool BigVehicleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 3b6f644614d9b..892b993a75348 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -58,10 +58,10 @@ NormalVehicleTracker::NormalVehicleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = autoware_universe_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -70,7 +70,7 @@ NormalVehicleTracker::NormalVehicleTracker( // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + velocity_deviation_threshold_ = autoware_universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -121,8 +121,8 @@ NormalVehicleTracker::NormalVehicleTracker( // Set motion limits { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware_universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -145,7 +145,7 @@ NormalVehicleTracker::NormalVehicleTracker( constexpr double p0_stddev_x = 1.0; // in object coordinate [m] constexpr double p0_stddev_y = 0.3; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + autoware_universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -164,14 +164,15 @@ NormalVehicleTracker::NormalVehicleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware_universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double p0_stddev_slip = + autoware_universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -424,7 +425,7 @@ bool NormalVehicleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 6c8198b51ecd0..005d3c15df4df 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -57,9 +57,9 @@ PedestrianTracker::PedestrianTracker( // Initialize parameters // measurement noise covariance - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = autoware_universe_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -86,18 +86,18 @@ PedestrianTracker::PedestrianTracker( // Set motion model parameters { - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = autoware_universe_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = autoware_universe_utils::deg2rad(30); // [rad/(s*s)] motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } // Set motion limits motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30.0 /* [deg/s] maximum turn rate */ + autoware_universe_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ ); // Set initial state @@ -121,7 +121,7 @@ PedestrianTracker::PedestrianTracker( constexpr double p0_stddev_x = 2.0; // in object coordinate [m] constexpr double p0_stddev_y = 2.0; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] + autoware_universe_utils::deg2rad(1000); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -140,9 +140,9 @@ PedestrianTracker::PedestrianTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s] + autoware_universe_utils::kmph2mps(120); // in object coordinate [m/s] constexpr double p0_stddev_wz = - tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s] + autoware_universe_utils::deg2rad(360); // in object coordinate [rad/s] vel_cov = std::pow(p0_stddev_vel, 2.0); wz_cov = std::pow(p0_stddev_wz, 2.0); } else { @@ -323,7 +323,7 @@ bool PedestrianTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index f15d5042a2316..52952c7820396 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -16,9 +16,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -67,8 +67,8 @@ UnknownTracker::UnknownTracker( // Set motion limits motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ - tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + autoware_universe_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + autoware_universe_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ ); // Set initial state @@ -107,8 +107,8 @@ UnknownTracker::UnknownTracker( } if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/s] - constexpr double p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vx = autoware_universe_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = autoware_universe_utils::kmph2mps(10); // [m/s] const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index ef8a45f608098..bf040d417e6bb 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -21,8 +21,8 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #define EIGEN_MPL2_ONLY #include @@ -60,8 +60,8 @@ void BicycleMotionModel::setDefaultParams() lr_min); // set motion limitations - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30.0; // [deg] maximum slip angle + constexpr double max_vel = autoware_universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30.0; // [deg] maximum slip angle setMotionLimits(max_vel, max_slip); // set prediction parameters @@ -79,13 +79,13 @@ void BicycleMotionModel::setMotionParams( // set process noise covariance parameters motion_params_.q_stddev_acc_long = q_stddev_acc_long; motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; - motion_params_.q_stddev_yaw_rate_min = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_min); - motion_params_.q_stddev_yaw_rate_max = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_max); + motion_params_.q_stddev_yaw_rate_min = autoware_universe_utils::deg2rad(q_stddev_yaw_rate_min); + motion_params_.q_stddev_yaw_rate_max = autoware_universe_utils::deg2rad(q_stddev_yaw_rate_max); motion_params_.q_cov_slip_rate_min = - std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_min), 2.0); + std::pow(autoware_universe_utils::deg2rad(q_stddev_slip_rate_min), 2.0); motion_params_.q_cov_slip_rate_max = - std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_max), 2.0); - motion_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(q_max_slip_angle); + std::pow(autoware_universe_utils::deg2rad(q_stddev_slip_rate_max), 2.0); + motion_params_.q_max_slip_angle = autoware_universe_utils::deg2rad(q_max_slip_angle); constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { @@ -103,7 +103,7 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); + motion_params_.max_slip = autoware_universe_utils::deg2rad(max_slip); } bool BicycleMotionModel::initialize( @@ -252,7 +252,7 @@ bool BicycleMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware_universe_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 9e8327e381057..af05590e3b05b 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -21,8 +21,8 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #define EIGEN_MPL2_ONLY #include @@ -40,17 +40,17 @@ CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger(" void CTRVMotionModel::setDefaultParams() { // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = autoware_universe_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = autoware_universe_utils::deg2rad(30); // [rad/(s*s)] setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); // set motion limitations - constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] maximum velocity - constexpr double max_wz = 30.0; // [deg] maximum yaw rate + constexpr double max_vel = autoware_universe_utils::kmph2mps(10); // [m/s] maximum velocity + constexpr double max_wz = 30.0; // [deg] maximum yaw rate setMotionLimits(max_vel, max_wz); // set prediction parameters @@ -74,7 +74,7 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); + motion_params_.max_wz = autoware_universe_utils::deg2rad(max_wz); } bool CTRVMotionModel::initialize( @@ -220,7 +220,7 @@ bool CTRVMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware_universe_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index a9ad03e7875c5..5ce944b0a79bb 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -21,8 +21,8 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #include @@ -49,8 +49,8 @@ void CVMotionModel::setDefaultParams() setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); // set motion limitations - constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum x velocity - constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum y velocity + constexpr double max_vx = autoware_universe_utils::kmph2mps(60); // [m/s] maximum x velocity + constexpr double max_vy = autoware_universe_utils::kmph2mps(60); // [m/s] maximum y velocity setMotionLimits(max_vx, max_vy); // set prediction parameters diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 7fb9bae7d9986..26fccca21aa22 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -17,10 +17,10 @@ #include "object_merger/data_association/data_association.hpp" +#include +#include +#include #include -#include -#include -#include #include @@ -84,10 +84,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node } overlapped_judge_param_; // debug publisher - std::unique_ptr processing_time_publisher_; - std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_association diff --git a/perception/object_merger/package.xml b/perception/object_merger/package.xml index 82a6399c09ba7..0340a477936b8 100644 --- a/perception/object_merger/package.xml +++ b/perception/object_merger/package.xml @@ -14,6 +14,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils eigen mussp object_recognition_utils @@ -21,7 +22,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp index 3abecd80178c0..634be73508a2d 100644 --- a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -14,10 +14,10 @@ #include "object_merger/data_association/data_association.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_merger/data_association/solver/gnn_solver.hpp" #include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -31,8 +31,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -133,7 +133,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware_universe_utils::calcDistance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index a4a74f5c763fb..cc1069321f6f8 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -14,9 +14,9 @@ #include "object_merger/node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -43,7 +43,7 @@ bool isUnknownObjectOverlapped( const double distance_threshold = distance_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); - const double sq_distance = tier4_autoware_utils::calcSquaredDistance2d( + const double sq_distance = autoware_universe_utils::calcSquaredDistance2d( unknown_object.kinematics.pose_with_covariance.pose, known_object.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; @@ -121,11 +121,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio // Debug publisher processing_time_publisher_ = - std::make_unique(this, "object_association_merger"); - stop_watch_ptr_ = std::make_unique>(); + std::make_unique(this, "object_association_merger"); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/object_velocity_splitter/package.xml b/perception/object_velocity_splitter/package.xml index a0ffe5e802801..32fd5b2ff6337 100644 --- a/perception/object_velocity_splitter/package.xml +++ b/perception/object_velocity_splitter/package.xml @@ -13,10 +13,10 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp index 2e496d7ee2ed6..4d635cf330fa6 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp @@ -14,7 +14,7 @@ #include "object_velocity_splitter/object_velocity_splitter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -74,7 +74,7 @@ void ObjectVelocitySplitterNode::onObjects(const DetectedObjects::ConstSharedPtr for (const auto & object : objects_data_->objects) { if ( - std::abs(tier4_autoware_utils::calcNorm( + std::abs(autoware_universe_utils::calcNorm( object.kinematics.twist_with_covariance.twist.linear)) < node_param_.velocity_threshold) { low_speed_objects.objects.emplace_back(object); } else { diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 0faa40255a1ca..bed7f84ab79a2 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -17,9 +17,9 @@ #include "pointcloud_preprocessor/filter.hpp" +#include #include #include -#include #include #include @@ -120,9 +120,9 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node // Debugger std::shared_ptr debugger_ptr_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; // ROS Parameters std::string map_frame_; diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index dafd73f38faef..0e920549eb728 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils autoware_vehicle_msgs diagnostic_updater image_transport @@ -37,7 +38,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_pcl_extensions diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index bbd138030d76f..37811f4f6fb08 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -14,10 +14,10 @@ #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" +#include +#include +#include #include -#include -#include -#include #include @@ -73,7 +73,7 @@ geometry_msgs::msg::PoseStamped getPoseStamped( RCLCPP_WARN_THROTTLE( rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what()); } - return tier4_autoware_utils::transform2pose(tf_stamped); + return autoware_universe_utils::transform2pose(tf_stamped); } boost::optional getCost( @@ -225,8 +225,8 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "occupancy_grid_map_outlier_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp index 19311559a3e7d..d09b1d9782604 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -20,10 +20,10 @@ #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include +#include #include #include -#include -#include #include #include @@ -85,8 +85,8 @@ class GridMapFusionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr fused_map_pub_; rclcpp::Publisher::SharedPtr single_frame_pub_; std::vector::SharedPtr> grid_map_subs_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; // Topics manager std::size_t num_input_topics_{1}; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 93486e0ca56af..7c3b7b3e4b1a3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -19,11 +19,11 @@ #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include +#include #include #include #include -#include -#include #include #include @@ -68,8 +68,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node rclcpp::Publisher::SharedPtr occupancy_grid_map_pub_; message_filters::Subscriber obstacle_pointcloud_sub_; message_filters::Subscriber raw_pointcloud_sub_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; std::shared_ptr tf2_{std::make_shared(get_clock())}; std::shared_ptr tf2_listener_{std::make_shared(*tf2_)}; diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index 3efed07535504..d8ae702ef60ca 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils eigen3_cmake_module grid_map_costmap_2d grid_map_msgs @@ -31,7 +32,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_autoware_utils visualization_msgs pointcloud_preprocessor diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index f191a23eb8e65..211479ed24e6b 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -142,8 +142,8 @@ GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) // debug tools { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 2722f2ef98249..f66ca3c7fc472 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -17,9 +17,9 @@ #include "probabilistic_occupancy_grid_map/cost_value.hpp" #include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include #include #include -#include #include @@ -55,9 +55,9 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); - constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); - constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); + constexpr double min_angle = autoware_universe_utils::deg2rad(-180.0); + constexpr double max_angle = autoware_universe_utils::deg2rad(180.0); + constexpr double angle_increment = autoware_universe_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index eae66eb90355b..0a16c488158f1 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -17,10 +17,10 @@ #include "probabilistic_occupancy_grid_map/cost_value.hpp" #include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include #include #include #include -#include #include @@ -56,9 +56,9 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); - constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); - constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); + constexpr double min_angle = autoware_universe_utils::deg2rad(-180.0); + constexpr double max_angle = autoware_universe_utils::deg2rad(180.0); + constexpr double angle_increment = autoware_universe_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 96afb0f2e852c..2f3a97c82ccc8 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -19,9 +19,9 @@ #include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" #include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include +#include #include -#include -#include #include @@ -119,8 +119,8 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "pointcloud_based_occupancy_grid_map"); diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 8009a503167ea..8e9a8431a4258 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -14,7 +14,7 @@ #include "probabilistic_occupancy_grid_map/utils/utils.hpp" -#include +#include #include @@ -50,7 +50,7 @@ void transformPointcloud( const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose, sensor_msgs::msg::PointCloud2 & output) { - const auto transform = tier4_autoware_utils::pose2transform(pose); + const auto transform = autoware_universe_utils::pose2transform(pose); Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); pcl_ros::transformPointCloud(tf_matrix, input, output); @@ -96,7 +96,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform( target_frame, source_header.frame_id, source_header.stamp, rclcpp::Duration::from_seconds(0.5)); - pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + pose = autoware_universe_utils::transform2pose(tf_stamped.transform); return pose; } @@ -108,7 +108,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); - pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + pose = autoware_universe_utils::transform2pose(tf_stamped.transform); return pose; } diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/radar_crossing_objects_noise_filter/package.xml index 2cfe2528a8bbd..0db40b24e809c 100644 --- a/perception/radar_crossing_objects_noise_filter/package.xml +++ b/perception/radar_crossing_objects_noise_filter/package.xml @@ -15,12 +15,12 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp index a3f4d2a6ce9d0..339cb8afa454b 100644 --- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp @@ -14,8 +14,8 @@ #include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include @@ -124,14 +124,14 @@ rcl_interfaces::msg::SetParametersResult RadarCrossingObjectsNoiseFilterNode::on bool RadarCrossingObjectsNoiseFilterNode::isNoise(const DetectedObject & object) { - const double velocity = - std::abs(tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear)); + const double velocity = std::abs( + autoware_universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear)); const double object_angle = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double object_position_angle = std::atan2( object.kinematics.pose_with_covariance.pose.position.y, object.kinematics.pose_with_covariance.pose.position.x); const double crossing_yaw = - tier4_autoware_utils::normalizeRadian(object_angle - object_position_angle); + autoware_universe_utils::normalizeRadian(object_angle - object_position_angle); if ( velocity > node_param_.velocity_threshold && diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp index a1c432d9f47b3..501edcbe84af8 100644 --- a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -48,9 +48,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) { rclcpp::init(0, nullptr); { - auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_universe_utils::createVector3(40.0, 30.0, 0.0); + auto position = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -81,9 +81,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_universe_utils::createVector3(40.0, 30.0, 0.0); + auto position = autoware_universe_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -112,9 +112,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_universe_utils::createVector3(24.0, 18.0, 0.0); + auto position = autoware_universe_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -143,9 +143,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_universe_utils::createVector3(24.0, 18.0, 0.0); + auto position = autoware_universe_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = autoware_universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp index 716d4ee8b05ff..ef6891c6c8944 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp @@ -15,8 +15,8 @@ #ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ #define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #define EIGEN_MPL2_ONLY #include #include @@ -34,12 +34,12 @@ namespace radar_fusion_to_detected_object { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::Point2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistWithCovariance; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; class RadarFusionToDetectedObject { diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 4fe51b1002b15..aa0711f1cc6ce 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -16,13 +16,13 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils eigen geometry_msgs message_filters rclcpp rclcpp_components std_msgs - tier4_autoware_utils ament_lint_common autoware_lint_common diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 6a170386917e7..85940b7f99f3d 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -15,8 +15,8 @@ #include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" -#include -#include +#include +#include #include @@ -31,12 +31,12 @@ namespace radar_fusion_to_detected_object { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::Point2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistWithCovariance; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; void RadarFusionToDetectedObject::setParam(const Param & param) { @@ -136,7 +136,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( bool RadarFusionToDetectedObject::hasTwistCovariance( const TwistWithCovariance & twist_with_covariance) { - using IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto covariance = twist_with_covariance.covariance; if (covariance[IDX::X_X] == 0.0 && covariance[IDX::Y_Y] == 0.0 && covariance[IDX::Z_Z] == 0.0) { return false; @@ -151,11 +151,11 @@ bool RadarFusionToDetectedObject::isYawCorrect( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance, const double & yaw_threshold) { - const double twist_yaw = tier4_autoware_utils::normalizeRadian( + const double twist_yaw = autoware_universe_utils::normalizeRadian( std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x)); - const double object_yaw = tier4_autoware_utils::normalizeRadian( + const double object_yaw = autoware_universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(twist_yaw - object_yaw); + const double diff_yaw = autoware_universe_utils::normalizeRadian(twist_yaw - object_yaw); if (std::abs(diff_yaw) < yaw_threshold) { return true; } else if (M_PI - yaw_threshold < std::abs(diff_yaw)) { @@ -174,10 +174,12 @@ RadarFusionToDetectedObject::filterRadarWithinObject( { std::vector outputs{}; - tier4_autoware_utils::Point2d object_size{object.shape.dimensions.x, object.shape.dimensions.y}; + autoware_universe_utils::Point2d object_size{ + object.shape.dimensions.x, object.shape.dimensions.y}; LinearRing2d object_box = createObject2dWithMargin(object_size, param_.bounding_box_margin); - object_box = tier4_autoware_utils::transformVector( - object_box, tier4_autoware_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); + object_box = autoware_universe_utils::transformVector( + object_box, + autoware_universe_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); for (const auto & radar : (*radars)) { Point2d radar_point{ @@ -212,10 +214,10 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( Eigen::Vector2d vec_min_distance(0.0, 0.0); if (param_.velocity_weight_min_distance > 0.0) { auto comp_func = [&](const RadarInput & a, const RadarInput & b) { - return tier4_autoware_utils::calcSquaredDistance2d( + return autoware_universe_utils::calcSquaredDistance2d( a.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position) < - tier4_autoware_utils::calcSquaredDistance2d( + autoware_universe_utils::calcSquaredDistance2d( b.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position); }; diff --git a/perception/radar_object_clustering/README.md b/perception/radar_object_clustering/README.md index 5133ddbdbb4ad..abd2b6f9d1bfe 100644 --- a/perception/radar_object_clustering/README.md +++ b/perception/radar_object_clustering/README.md @@ -83,13 +83,13 @@ These are used in `isSameObject` function as below. bool RadarObjectClusteringNode::isSameObject( const DetectedObject & object_1, const DetectedObject & object_2) { - const double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double angle_diff = std::abs(autoware_universe_utils::normalizeRadian( tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); const double velocity_diff = std::abs( object_1.kinematics.twist_with_covariance.twist.linear.x - object_2.kinematics.twist_with_covariance.twist.linear.x); - const double distance = tier4_autoware_utils::calcDistance2d( + const double distance = autoware_universe_utils::calcDistance2d( object_1.kinematics.pose_with_covariance.pose.position, object_2.kinematics.pose_with_covariance.pose.position); diff --git a/perception/radar_object_clustering/package.xml b/perception/radar_object_clustering/package.xml index 44d99032380cf..c7666d2c5701e 100644 --- a/perception/radar_object_clustering/package.xml +++ b/perception/radar_object_clustering/package.xml @@ -15,13 +15,13 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs object_recognition_utils rclcpp rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp index a659a1411dbf9..1681260c711b2 100644 --- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -14,9 +14,9 @@ #include "radar_object_clustering/radar_object_clustering_node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -179,13 +179,13 @@ void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr bool RadarObjectClusteringNode::isSameObject( const DetectedObject & object_1, const DetectedObject & object_2) { - const double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double angle_diff = std::abs(autoware_universe_utils::normalizeRadian( tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); const double velocity_diff = std::abs( object_1.kinematics.twist_with_covariance.twist.linear.x - object_2.kinematics.twist_with_covariance.twist.linear.x); - const double distance = tier4_autoware_utils::calcDistance2d( + const double distance = autoware_universe_utils::calcDistance2d( object_1.kinematics.pose_with_covariance.pose.position, object_2.kinematics.pose_with_covariance.pose.position); diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp index aaffea3d5c3ab..e1c33c022e427 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp @@ -34,9 +34,9 @@ #include #endif +#include +#include #include -#include -#include #include diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 4eb09a1def6a2..c3c4b33946b56 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -15,6 +15,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils eigen glog kalman_filter @@ -26,7 +27,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs yaml-cpp diff --git a/perception/radar_object_tracker/src/data_association/data_association.cpp b/perception/radar_object_tracker/src/data_association/data_association.cpp index bd1860e6b0136..ddfb0c3e8f76d 100644 --- a/perception/radar_object_tracker/src/data_association/data_association.cpp +++ b/perception/radar_object_tracker/src/data_association/data_association.cpp @@ -55,8 +55,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { const double measurement_yaw = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + autoware_universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware_universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -203,7 +203,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware_universe_utils::calcDistance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -221,7 +221,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = tier4_autoware_utils::getArea(measurement_object.shape); + const double area = autoware_universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) { passed_gate = false; } diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index c2088858b76c5..0548002606d5d 100644 --- a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -20,8 +20,8 @@ #include "radar_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #include #include @@ -209,7 +209,7 @@ void ConstantTurnRateMotionTracker::loadDefaultModelParameters(const std::string // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vx_ = autoware_universe_utils::kmph2mps(max_speed_kmph); // [m/s] } bool ConstantTurnRateMotionTracker::predict(const rclcpp::Time & time) @@ -393,7 +393,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose( Eigen::MatrixXd Y_yaw = Eigen::MatrixXd::Zero(1, 1); const auto yaw = [&] { - auto obj_yaw = tier4_autoware_utils::normalizeRadian( + auto obj_yaw = autoware_universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); while (M_PI_2 <= yaw_state - obj_yaw) { obj_yaw = obj_yaw + M_PI; @@ -617,7 +617,7 @@ bool ConstantTurnRateMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index ce9ddef6ea993..754ff88861b0d 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -20,8 +20,8 @@ #include "radar_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #include #include @@ -237,8 +237,8 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] - max_vy_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [rad/s] + max_vx_ = autoware_universe_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vy_ = autoware_universe_utils::kmph2mps(max_speed_kmph); // [rad/s] } bool LinearMotionTracker::predict(const rclcpp::Time & time) @@ -682,7 +682,7 @@ bool LinearMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp index 57d7b5a21fa94..fa1acda54e4fe 100644 --- a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp +++ b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -81,10 +81,10 @@ bool checkCloseLaneletCondition( double object_motion_yaw = object_yaw; bool velocity_is_reverted = object.kinematics.twist_with_covariance.twist.linear.x < 0.0; if (velocity_is_reverted) { - object_motion_yaw = tier4_autoware_utils::normalizeRadian(object_yaw + M_PI); + object_motion_yaw = autoware_universe_utils::normalizeRadian(object_yaw + M_PI); } const double delta_yaw = object_motion_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw > max_angle_diff_from_lane) { @@ -134,14 +134,14 @@ bool hasValidVelocityDirectionToLanelet( const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); const double object_vel_yaw_global = - tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); + autoware_universe_utils::normalizeRadian(object_yaw + object_vel_yaw); const double object_vel = std::hypot(object_vel_x, object_vel_y); for (const auto & lanelet : lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_vel_yaw_global - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_universe_utils::normalizeRadian(delta_yaw); const double lane_vel = object_vel * std::sin(normalized_delta_yaw); if (std::fabs(lane_vel) < max_lateral_velocity) { diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 59c9e5e579772..8db732245b1fd 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -15,8 +15,8 @@ #ifndef RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ #define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" #include "autoware_perception_msgs/msg/object_classification.hpp" @@ -64,7 +64,7 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_radar_{}; rclcpp::Subscription::SharedPtr sub_odometry_{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onRadarTracks(const RadarTracks::ConstSharedPtr msg); diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml index f4b8ea496897f..ad0257dbbd726 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/radar_tracks_msgs_converter/package.xml @@ -16,6 +16,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils geometry_msgs nav_msgs radar_msgs @@ -24,7 +25,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 234f430d6fb81..0caf29df2735e 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -14,9 +14,9 @@ #include "radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include @@ -92,7 +92,7 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1)); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // Publisher pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1); @@ -261,7 +261,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() } // yaw angle (vehicle heading) is obtained from ground velocity - double yaw = tier4_autoware_utils::normalizeRadian( + double yaw = autoware_universe_utils::normalizeRadian( std::atan2(compensated_velocity.y, compensated_velocity.x)); // kinematics setting @@ -273,7 +273,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() // velocity of object is defined in the object coordinate // heading is obtained from ground velocity kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware_universe_utils::createQuaternionFromYaw(yaw); // longitudinal velocity is the length of the velocity vector // lateral velocity is zero, use default value kinematics.twist_with_covariance.twist.linear.x = std::sqrt( @@ -334,8 +334,8 @@ geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocityEgoM std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware_universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; @@ -351,8 +351,8 @@ std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware_universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array twist_covariance{}; twist_covariance[POSE_IDX::X_X] = radar_track.velocity_covariance[RADAR_IDX::X_X]; twist_covariance[POSE_IDX::X_Y] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; @@ -368,8 +368,8 @@ std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatri std::array RadarTracksMsgsConverterNode::convertAccelerationCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware_universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array acceleration_covariance{}; acceleration_covariance[POSE_IDX::X_X] = radar_track.acceleration_covariance[RADAR_IDX::X_X]; acceleration_covariance[POSE_IDX::X_Y] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; diff --git a/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp b/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp index 86a790e208012..5e010b21478fc 100644 --- a/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp +++ b/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp @@ -17,9 +17,9 @@ #include "detected_object_validation/utils/utils.hpp" +#include +#include #include -#include -#include #include @@ -68,9 +68,9 @@ class LowIntensityClusterFilter : public rclcpp::Node utils::FilterTargetLabel filter_target_; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace low_intensity_cluster_filter diff --git a/perception/raindrop_cluster_filter/package.xml b/perception/raindrop_cluster_filter/package.xml index 492b789239d54..bff3f71db6f04 100644 --- a/perception/raindrop_cluster_filter/package.xml +++ b/perception/raindrop_cluster_filter/package.xml @@ -12,6 +12,7 @@ ament_cmake autoware_cmake + autoware_universe_utils detected_object_validation lanelet2_extension message_filters @@ -24,7 +25,6 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp index 5c47b61eaa0a3..cae09d3cb20e7 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp @@ -14,8 +14,8 @@ #include "raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp" +#include #include -#include #include @@ -52,8 +52,8 @@ LowIntensityClusterFilter::LowIntensityClusterFilter(const rclcpp::NodeOptions & "output/objects", rclcpp::QoS{1}); // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "low_intensity_cluster_filter_node"); @@ -85,8 +85,8 @@ void LowIntensityClusterFilter::objectCallback( geometry_msgs::msg::Pose max_pose; max_pose.position.x = max_x_; max_pose.position.y = max_y_; - auto min_ranged_transformed = tier4_autoware_utils::transformPose(min_range, transform_stamp); - auto max_range_transformed = tier4_autoware_utils::transformPose(max_pose, transform_stamp); + auto min_ranged_transformed = autoware_universe_utils::transformPose(min_range, transform_stamp); + auto max_range_transformed = autoware_universe_utils::transformPose(max_pose, transform_stamp); for (const auto & feature_object : input_msg->feature_objects) { const auto & object = feature_object.object; const auto & label = object.classification.front().label; diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index af42400dcc065..953736e0924ce 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -14,7 +14,7 @@ #include "shape_estimation/corrector/utils.hpp" -#include +#include #include #include @@ -246,9 +246,9 @@ bool correctWithDefaultValue( // correct to set long length is x, short length is y if (shape.dimensions.x < shape.dimensions.y) { - geometry_msgs::msg::Vector3 rpy = tier4_autoware_utils::getRPY(pose.orientation); + geometry_msgs::msg::Vector3 rpy = autoware_universe_utils::getRPY(pose.orientation); rpy.z = rpy.z + M_PI_2; - pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); + pose.orientation = autoware_universe_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); double temp = shape.dimensions.x; shape.dimensions.x = shape.dimensions.y; shape.dimensions.y = temp; diff --git a/perception/shape_estimation/package.xml b/perception/shape_estimation/package.xml index e91a399baa076..8eab542036177 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/shape_estimation/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils eigen libopencv-dev libpcl-all-dev @@ -21,7 +22,6 @@ tf2 tf2_eigen tf2_geometry_msgs - tier4_autoware_utils tier4_perception_msgs ament_cmake_gtest diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index c20308ca0b6d1..119b0d22b3f7d 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -14,8 +14,8 @@ #include "shape_estimation/shape_estimator.hpp" +#include #include -#include #include @@ -54,11 +54,13 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); processing_time_publisher_ = - std::make_unique(this, "shape_estimation"); - stop_watch_ptr_ = std::make_unique>(); + std::make_unique(this, "shape_estimation"); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } static autoware_perception_msgs::msg::ObjectClassification::_label_type get_label( @@ -112,7 +114,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared if (use_vehicle_reference_yaw_ && is_vehicle) { ref_yaw_info = ReferenceYawInfo{ static_cast(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)), - tier4_autoware_utils::deg2rad(10)}; + autoware_universe_utils::deg2rad(10)}; } if (use_vehicle_reference_shape_size_ && is_vehicle) { ref_shape_size_info = ReferenceShapeSizeInfo{object.shape, ReferenceShapeSizeInfo::Mode::Min}; diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index a3788668ef6ae..db2a8b9ddaedd 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -17,10 +17,10 @@ #include "shape_estimation/shape_estimator.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -35,11 +35,11 @@ class ShapeEstimationNode : public rclcpp::Node // ros rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp index 223272b64d716..73ccd33464d81 100644 --- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp +++ b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp @@ -15,8 +15,8 @@ #ifndef SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ #define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" @@ -47,7 +47,7 @@ class SimpleObjectMergerNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_objects_{}; std::vector::SharedPtr> sub_objects_array{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number); diff --git a/perception/simple_object_merger/package.xml b/perception/simple_object_merger/package.xml index 4586efe53f2e7..487155bffd102 100644 --- a/perception/simple_object_merger/package.xml +++ b/perception/simple_object_merger/package.xml @@ -13,10 +13,10 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index fabf06595970c..af8f02caef72b 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -98,7 +98,7 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ } // Subscriber - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_objects_array.resize(input_topic_size); objects_data_.resize(input_topic_size); diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp index fc396ae4b3473..885852106a82c 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -15,12 +15,12 @@ #ifndef TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ #define TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ +#include +#include #include #include #include #include -#include -#include #include #include @@ -63,8 +63,8 @@ class TrtYoloXNode : public rclcpp::Node LabelMap label_map_; std::unique_ptr trt_yolox_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index c1c4c42741cee..4eb266445489e 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -31,9 +31,9 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) { { stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); debug_publisher_ = - std::make_unique(this, "tensorrt_yolox"); + std::make_unique(this, "tensorrt_yolox"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 7fe4d3a06decc..be03ec3868af9 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -19,10 +19,10 @@ #include "tracking_object_merger/utils/tracker_state.hpp" #include "tracking_object_merger/utils/utils.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -86,8 +86,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // debug object publisher rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> @@ -106,7 +106,7 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // tracker default settings TrackerStateParameter tracker_state_parameter_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // merge policy (currently not used) struct diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp index be53f69ff74cb..1778e09da5cd9 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -17,8 +17,8 @@ #ifndef TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ #define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ -// #include -#include "tier4_autoware_utils/geometry/geometry.hpp" +// #include +#include "autoware/universe_utils/geometry/geometry.hpp" #include diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml index d5904571cec6e..370f5beb3e3b3 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/tracking_object_merger/package.xml @@ -14,6 +14,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils eigen glog mussp @@ -22,7 +23,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp index 21bb1b46cb14a..ae4c05013b7db 100644 --- a/perception/tracking_object_merger/src/data_association/data_association.cpp +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -14,8 +14,8 @@ #include "tracking_object_merger/data_association/data_association.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tracking_object_merger/data_association/solver/gnn_solver.hpp" #include "tracking_object_merger/utils/utils.hpp" @@ -31,8 +31,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = autoware_universe_utils::normalizeRadian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -183,7 +183,7 @@ double DataAssociation::calcScoreBetweenObjects( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware_universe_utils::calcDistance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index 30762d4a21231..d1bc61d90edac 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -155,12 +155,14 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("radar-radar", data_association_map_); // debug publisher - processing_time_publisher_ = - std::make_unique(this, "decorative_object_merger_node"); - stop_watch_ptr_ = std::make_unique>(); + processing_time_publisher_ = std::make_unique( + this, "decorative_object_merger_node"); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 343c9d9711032..7d8b5a4cc02f3 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -280,7 +280,7 @@ bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const Track // diff of motion yaw angle const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw); const auto normalized_motion_yaw_diff = - tier4_autoware_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi + autoware_universe_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi // evaluate if motion yaw angle is same constexpr double yaw_threshold = M_PI / 4.0; // 45 deg if (std::abs(normalized_motion_yaw_diff) < yaw_threshold) { @@ -305,7 +305,7 @@ bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); // calc yaw diff const auto yaw_diff = std::fabs(main_yaw - sub_yaw); - const auto normalized_yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_diff); // -pi ~ pi + const auto normalized_yaw_diff = autoware_universe_utils::normalizeRadian(yaw_diff); // -pi ~ pi // evaluate if yaw is reverted constexpr double yaw_threshold = M_PI / 2.0; // 90 deg if (std::abs(normalized_yaw_diff) >= yaw_threshold) { diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index de0f62829cf18..e72cac75cd809 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -15,6 +15,7 @@ autoware_map_msgs autoware_planning_msgs + autoware_universe_utils geometry_msgs image_geometry lanelet2_extension @@ -25,7 +26,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index 465ab5f85d9a7..7224db018ccff 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -14,11 +14,11 @@ #include "traffic_light_map_based_detector/node.hpp" +#include +#include #include #include #include -#include -#include #include #include @@ -512,9 +512,9 @@ void MapBasedDetector::getVisibleTrafficLights( double max_angle_range; if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { max_angle_range = - tier4_autoware_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); + autoware_universe_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); } else { - max_angle_range = tier4_autoware_utils::deg2rad(config_.car_traffic_light_max_angle_range); + max_angle_range = autoware_universe_utils::deg2rad(config_.car_traffic_light_max_angle_range); } // traffic light bottom left const auto & tl_bl = traffic_light.front(); @@ -530,7 +530,7 @@ void MapBasedDetector::getVisibleTrafficLights( } // check angle range - const double tl_yaw = tier4_autoware_utils::normalizeRadian( + const double tl_yaw = autoware_universe_utils::normalizeRadian( std::atan2(tl_br.y() - tl_bl.y(), tl_br.x() - tl_bl.x()) + M_PI_2); // get direction of z axis @@ -538,7 +538,7 @@ void MapBasedDetector::getVisibleTrafficLights( tf2::Matrix3x3 camera_rotation_matrix(tf_map2camera.getRotation()); camera_z_dir = camera_rotation_matrix * camera_z_dir; double camera_yaw = std::atan2(camera_z_dir.y(), camera_z_dir.x()); - camera_yaw = tier4_autoware_utils::normalizeRadian(camera_yaw); + camera_yaw = autoware_universe_utils::normalizeRadian(camera_yaw); if (!isInAngleRange(tl_yaw, camera_yaw, max_angle_range)) { continue; } diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp index 7e2825197f3a7..c14a5d56dc5d4 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp @@ -16,10 +16,10 @@ #ifndef TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__OCCLUSION_PREDICTOR_HPP_ #define TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__OCCLUSION_PREDICTOR_HPP_ +#include +#include #include #include -#include -#include #include #include diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 47077ce342621..9a701548092d9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_map_msgs + autoware_universe_utils geometry_msgs image_geometry lanelet2_extension @@ -24,7 +25,6 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils tier4_perception_msgs traffic_light_utils diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index ddd4e52d5248a..1a2eaa6c4d941 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -14,11 +14,11 @@ #include "traffic_light_occlusion_predictor/nodelet.hpp" +#include #include #include #include #include -#include #include #include diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 6f96913420bad..8a1bea3303c67 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -102,7 +102,7 @@ void CloudOcclusionPredictor::predict( pcl::PointCloud cloud_camera; // points within roi pcl::PointCloud cloud_roi; - tier4_autoware_utils::transformPointCloudFromROSMsg(*cloud_msg, cloud_camera, camera2cloud); + autoware_universe_utils::transformPointCloudFromROSMsg(*cloud_msg, cloud_camera, camera2cloud); filterCloud(cloud_camera, roi_tls, roi_brs, cloud_roi); diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 81c2c41762e2d..11f05ad126bc1 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -31,7 +31,7 @@ #ifndef AUTOWARE__FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ #define AUTOWARE__FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" #include #include @@ -163,7 +163,7 @@ class FreespacePlannerNode : public rclcpp::Node TransformStamped getTransform(const std::string & from, const std::string & to); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::freespace_planner diff --git a/planning/autoware_freespace_planner/package.xml b/planning/autoware_freespace_planner/package.xml index a5ed76d7d27b6..b098c7337954e 100644 --- a/planning/autoware_freespace_planner/package.xml +++ b/planning/autoware_freespace_planner/package.xml @@ -19,6 +19,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs motion_utils @@ -28,7 +29,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index b5e5d8c59cadb..ca012e7d85316 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -32,8 +32,8 @@ #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include #include -#include #include #include @@ -143,7 +143,7 @@ Trajectory getPartialTrajectory( double calcDistance2d(const Trajectory & trajectory, const Pose & pose) { const auto idx = motion_utils::findNearestIndex(trajectory.points, pose.position); - return tier4_autoware_utils::calcDistance2d(trajectory.points.at(idx), pose); + return autoware_universe_utils::calcDistance2d(trajectory.points.at(idx), pose); } Pose transformPose(const Pose & pose, const TransformStamped & transform) @@ -287,7 +287,7 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() @@ -395,7 +395,7 @@ bool FreespacePlannerNode::isPlanRequired() void FreespacePlannerNode::updateTargetIndex() { const auto is_near_target = - tier4_autoware_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < + autoware_universe_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < node_param_.th_arrived_distance_m; const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); 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 cb2e5122f8b4b..bca08f0d11c58 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 diff --git a/planning/autoware_freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml index 61173f0a877e4..05e906c998096 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_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs @@ -25,7 +26,6 @@ std_msgs tf2 tf2_geometry_msgs - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index c153d4078be6a..dbab68be5f8be 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,15 +14,15 @@ #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" -#include -#include +#include +#include #include namespace autoware::freespace_planning_algorithms { -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::normalizeRadian; +using autoware_universe_utils::createQuaternionFromYaw; +using autoware_universe_utils::normalizeRadian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) @@ -97,7 +97,7 @@ double PlannerWaypoints::compute_length() const for (size_t i = 0; i < waypoints.size() - 1; ++i) { const auto pose_a = waypoints.at(i); const auto pose_b = waypoints.at(i + 1); - total_cost += tier4_autoware_utils::calcDistance2d(pose_a.pose, pose_b.pose); + total_cost += autoware_universe_utils::calcDistance2d(pose_a.pose, pose_b.pose); } return total_cost; } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 1b7870d092208..5aa7f9889ccf8 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -14,8 +14,8 @@ #include "autoware/freespace_planning_algorithms/astar_search.hpp" -#include -#include +#include +#include #include @@ -42,7 +42,7 @@ double calcReedsSheppDistance( void setYaw(geometry_msgs::msg::Quaternion * orientation, const double yaw) { - *orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + *orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); } geometry_msgs::msg::Pose calcRelativePose( @@ -209,7 +209,7 @@ double AstarSearch::estimateCost(const geometry_msgs::msg::Pose & pose) const total_cost += calcReedsSheppDistance(pose, goal_pose_, radius) * astar_param_.distance_heuristic_weight; } else { - total_cost += tier4_autoware_utils::calcDistance2d(pose, goal_pose_) * + total_cost += autoware_universe_utils::calcDistance2d(pose, goal_pose_) * astar_param_.distance_heuristic_weight; } return total_cost; @@ -332,7 +332,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const const double lateral_goal_range = planner_common_param_.lateral_goal_range / 2.0; const double longitudinal_goal_range = planner_common_param_.longitudinal_goal_range / 2.0; const double goal_angle = - tier4_autoware_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); + autoware_universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); const auto relative_pose = calcRelativePose(goal_pose_, node2pose(node)); @@ -348,7 +348,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const } const auto angle_diff = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + autoware_universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); if (std::abs(angle_diff) > goal_angle) { return false; } @@ -363,7 +363,7 @@ geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const pose_local.position.x = node.x; pose_local.position.y = node.y; pose_local.position.z = goal_pose_.position.z; - pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); + pose_local.orientation = autoware_universe_utils::createQuaternionFromYaw(node.theta); return pose_local; } diff --git a/planning/autoware_mission_planner/package.xml b/planning/autoware_mission_planner/package.xml index 0a0491e75dd95..6fb7cca836fd8 100644 --- a/planning/autoware_mission_planner/package.xml +++ b/planning/autoware_mission_planner/package.xml @@ -21,6 +21,7 @@ autoware_map_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension @@ -30,7 +31,6 @@ rclcpp_components tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs ament_lint_auto diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index e1c41eee2fb20..275d5cd825787 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -16,16 +16,16 @@ #include "utility_functions.hpp" +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include -#include #include #include @@ -190,13 +190,13 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } const std_msgs::msg::ColorRGBA cl_route = - tier4_autoware_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15); + autoware_universe_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15); const std_msgs::msg::ColorRGBA cl_ll_borders = - tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); + autoware_universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); const std_msgs::msg::ColorRGBA cl_end = - tier4_autoware_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05); + autoware_universe_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05); const std_msgs::msg::ColorRGBA cl_goal = - tier4_autoware_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05); + autoware_universe_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05); visualization_msgs::msg::MarkerArray route_marker_array; insert_marker_array( @@ -216,27 +216,27 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( - tier4_autoware_utils::LinearRing2d goal_footprint) const + autoware_universe_utils::LinearRing2d goal_footprint) const { visualization_msgs::msg::MarkerArray msg; - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); + autoware_universe_utils::createMarkerScale(0.05, 0.0, 0.0), + autoware_universe_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); marker.lifetime = rclcpp::Duration::from_seconds(2.5); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); + autoware_universe_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); + autoware_universe_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); + autoware_universe_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); + autoware_universe_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); + autoware_universe_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); + autoware_universe_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -247,7 +247,7 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( bool DefaultPlanner::check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelet & combined_prev_lanelet, - const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const autoware_universe_utils::Polygon2d & goal_footprint, double & next_lane_length, const double search_margin) { // check if goal footprint is in current lane @@ -302,8 +302,8 @@ bool DefaultPlanner::is_goal_valid( shoulder_lanelets, goal, &closest_shoulder_lanelet)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + const auto angle_diff = autoware_universe_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = autoware_universe_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -332,8 +332,8 @@ bool DefaultPlanner::is_goal_valid( } const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - tier4_autoware_utils::LinearRing2d goal_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(goal)); + autoware_universe_utils::LinearRing2d goal_footprint = + transformVector(local_vehicle_footprint, autoware_universe_utils::pose2transform(goal)); pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); @@ -357,9 +357,9 @@ bool DefaultPlanner::is_goal_valid( if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const auto angle_diff = autoware_universe_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + const double th_angle = autoware_universe_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index 63bfac97672ea..0b7d889a24f83 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -51,7 +51,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void updateRoute(const PlannerPlugin::LaneletRoute & route) override; void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; - MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; + MarkerArray visualize_debug_footprint( + autoware_universe_utils::LinearRing2d goal_footprint_) const; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: @@ -85,7 +86,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin bool check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelet & combined_prev_lanelet, - const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const autoware_universe_utils::Polygon2d & goal_footprint, double & next_lane_length, const double search_margin = 2.0); /** diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 1f3b2077cc5eb..904e618f825fb 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -21,10 +21,10 @@ #include #include -tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( - tier4_autoware_utils::LinearRing2d footprint) +autoware_universe_utils::Polygon2d convert_linear_ring_to_polygon( + autoware_universe_utils::LinearRing2d footprint) { - tier4_autoware_utils::Polygon2d footprint_polygon; + autoware_universe_utils::Polygon2d footprint_polygon; boost::geometry::append(footprint_polygon.outer(), footprint[0]); boost::geometry::append(footprint_polygon.outer(), footprint[1]); boost::geometry::append(footprint_polygon.outer(), footprint[2]); @@ -120,7 +120,7 @@ geometry_msgs::msg::Pose convertBasicPoint3dToPose( pose.position.y = point.y(); pose.position.z = point.z(); - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + pose.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); return pose; } diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index 4ff7cf16beb70..04f235a58206f 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -16,9 +16,9 @@ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #include +#include #include #include -#include #include #include @@ -42,8 +42,8 @@ bool exists(const std::vector & vectors, const T & item) return false; } -tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( - tier4_autoware_utils::LinearRing2d footprint); +autoware_universe_utils::Polygon2d convert_linear_ring_to_polygon( + autoware_universe_utils::LinearRing2d footprint); void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); diff --git a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp index 54285bd1491f9..b0af480d10357 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp @@ -14,9 +14,9 @@ #include "arrival_checker.hpp" -#include -#include -#include +#include +#include +#include #include @@ -26,7 +26,7 @@ namespace autoware::mission_planner ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node) { const double angle_deg = node->declare_parameter("arrival_check_angle_deg"); - angle_ = tier4_autoware_utils::deg2rad(angle_deg); + angle_ = autoware_universe_utils::deg2rad(angle_deg); distance_ = node->declare_parameter("arrival_check_distance"); duration_ = node->declare_parameter("arrival_check_duration"); } @@ -67,14 +67,14 @@ bool ArrivalChecker::is_arrived(const PoseStamped & pose) const } // Check distance. - if (distance_ < tier4_autoware_utils::calcDistance2d(pose.pose, goal.pose)) { + if (distance_ < autoware_universe_utils::calcDistance2d(pose.pose, goal.pose)) { return false; } // Check angle. const double yaw_pose = tf2::getYaw(pose.pose.orientation); const double yaw_goal = tf2::getYaw(goal.pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_pose - yaw_goal); + const double yaw_diff = autoware_universe_utils::normalizeRadian(yaw_pose - yaw_goal); if (angle_ < std::fabs(yaw_diff)) { return false; } diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index 0500fa07ec31f..fc90b91f6d996 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -78,7 +78,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto period = rclcpp::Rate(10).period(); data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void MissionPlanner::publish_pose_log(const Pose & pose, const std::string & pose_type) diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index 77896adde331d..ab108d07918d2 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -16,13 +16,13 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include #include +#include #include #include -#include #include #include @@ -85,7 +85,7 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_reroute_availability_{this, "~/input/reroute_availability"}; rclcpp::Subscription::SharedPtr sub_vector_map_; @@ -132,7 +132,7 @@ class MissionPlanner : public rclcpp::Node double minimum_reroute_length_; bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::mission_planner diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp index c305b5d2e2732..e228b34cb9f5d 100644 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ #include "autoware/objects_of_interest_marker_interface/marker_data.hpp" -#include +#include #include diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp index fcf91db9e794b..ace32e91287a4 100644 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp @@ -17,11 +17,11 @@ #include "autoware/objects_of_interest_marker_interface/coloring.hpp" #include "autoware/objects_of_interest_marker_interface/marker_data.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/planning/autoware_objects_of_interest_marker_interface/package.xml b/planning/autoware_objects_of_interest_marker_interface/package.xml index a7701d159e824..8b62865ddace5 100644 --- a/planning/autoware_objects_of_interest_marker_interface/package.xml +++ b/planning/autoware_objects_of_interest_marker_interface/package.xml @@ -15,10 +15,10 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp std_msgs - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp index 88a7b9bf3e4df..8dba7731649b4 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp @@ -22,7 +22,7 @@ std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float a const float g = static_cast((code << 48) >> 56) / 255.0; const float b = static_cast((code << 56) >> 56) / 255.0; - return tier4_autoware_utils::createMarkerColor(r, g, b, alpha); + return autoware_universe_utils::createMarkerColor(r, g, b, alpha); } } // namespace diff --git a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp index a01542b013e78..4cbf6924abf36 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp @@ -20,8 +20,8 @@ using geometry_msgs::msg::Point; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerScale; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerScale; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -66,9 +66,9 @@ Marker createCircleMarker( for (size_t i = 0; i < num_points; ++i) { Point point; const double ratio = static_cast(i) / static_cast(num_points); - const double theta = 2 * tier4_autoware_utils::pi * ratio; - point.x = data.pose.position.x + radius * tier4_autoware_utils::cos(theta); - point.y = data.pose.position.y + radius * tier4_autoware_utils::sin(theta); + const double theta = 2 * autoware_universe_utils::pi * ratio; + point.x = data.pose.position.x + radius * autoware_universe_utils::cos(theta); + point.y = data.pose.position.y + radius * autoware_universe_utils::sin(theta); point.z = data.pose.position.z + height + height_offset; marker.points.push_back(point); } diff --git a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 881f3a8fcc6b8..253d2ec05c246 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -14,9 +14,9 @@ #include "autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" -#include -#include -#include +#include +#include +#include namespace autoware::objects_of_interest_marker_interface { diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index 8efc879727883..d0f78e695c8ce 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -16,12 +16,12 @@ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/uuid_helper.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include @@ -63,7 +63,7 @@ struct Obstacle twist(object.kinematics.initial_twist_with_covariance.twist), twist_reliable(true), classification(object.classification.at(0)), - uuid(tier4_autoware_utils::toHexString(object.object_id)), + uuid(autoware_universe_utils::toHexString(object.object_id)), shape(object.shape), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) @@ -74,7 +74,7 @@ struct Obstacle } } - Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } + Polygon2d toPolygon() const { return autoware_universe_utils::toPolygon2d(pose, shape); } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. geometry_msgs::msg::Pose pose; // interpolated with the current stamp @@ -198,33 +198,33 @@ struct LongitudinalInfo void onParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam(parameters, "normal.max_accel", max_accel); - tier4_autoware_utils::updateParam(parameters, "normal.min_accel", min_accel); - tier4_autoware_utils::updateParam(parameters, "normal.max_jerk", max_jerk); - tier4_autoware_utils::updateParam(parameters, "normal.min_jerk", min_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); - tier4_autoware_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); - tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam(parameters, "normal.max_accel", max_accel); + autoware_universe_utils::updateParam(parameters, "normal.min_accel", min_accel); + autoware_universe_utils::updateParam(parameters, "normal.max_jerk", max_jerk); + autoware_universe_utils::updateParam(parameters, "normal.min_jerk", min_jerk); + autoware_universe_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); + autoware_universe_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); + autoware_universe_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); + autoware_universe_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + autoware_universe_utils::updateParam( parameters, "common.slow_down_min_accel", slow_down_min_accel); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.slow_down_min_jerk", slow_down_min_jerk); - tier4_autoware_utils::updateParam(parameters, "common.idling_time", idling_time); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam(parameters, "common.idling_time", idling_time); + autoware_universe_utils::updateParam( parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.safe_distance_margin", safe_distance_margin); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold); } @@ -265,7 +265,7 @@ struct DebugData MarkerArray stop_wall_marker; MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; - std::vector detection_polygons; + std::vector detection_polygons; }; struct EgoNearestParam diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 32164981eaf99..5d0d99a742d1c 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -19,13 +19,13 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "signal_processing/lowpass_filter_1d.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include #include -#include #include #include @@ -145,11 +145,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr traj_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber objects_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; - tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; // Vehicle Parameters @@ -161,7 +161,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node PlanningAlgorithm planning_algorithm_; // stop watch - mutable tier4_autoware_utils::StopWatch< + mutable autoware_universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; mutable std::shared_ptr debug_data_ptr_{nullptr}; @@ -274,9 +274,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector prev_closest_stop_obstacles_{}; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_planning diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 355deb7cc87c8..e342ed3b7495e 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -19,9 +19,9 @@ #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -125,7 +125,7 @@ class PlannerInterface bool suppress_sudden_obstacle_stop_; // stop watch - tier4_autoware_utils::StopWatch< + autoware_universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; @@ -303,29 +303,29 @@ class PlannerInterface if (obstacle_to_param_struct_map.count(label + "." + movement_postfix) < 1) continue; auto & param_by_obstacle_label = obstacle_to_param_struct_map.at(label + "." + movement_postfix); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin", param_by_obstacle_label.max_lat_margin); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".min_lat_margin", param_by_obstacle_label.min_lat_margin); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".max_ego_velocity", param_by_obstacle_label.max_ego_velocity); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".min_ego_velocity", param_by_obstacle_label.min_ego_velocity); } } // common parameters - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down.lpf_gain_slow_down_vel", lpf_gain_slow_down_vel); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down.lpf_gain_lat_dist", lpf_gain_lat_dist); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } @@ -385,15 +385,15 @@ class PlannerInterface if (type_str == "default") { continue; } - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, param_prefix + type_str + ".sudden_object_acc_threshold", param.sudden_object_acc_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, param_prefix + type_str + ".sudden_object_dist_threshold", param.sudden_object_dist_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); param.sudden_object_acc_threshold = diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp index ec6102a3e9807..e9c661d575733 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp @@ -17,8 +17,8 @@ #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include @@ -30,8 +30,8 @@ namespace polygon_utils { namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; Polygon2d createOneStepPolygon( const std::vector & last_poses, diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 673a470decdb1..bc9f6070e159d 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -60,7 +60,7 @@ using tier4_planning_msgs::msg::VelocityLimitClearCommand; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index e229f5c54c5ed..ceb5da13882bd 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -16,8 +16,8 @@ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ #include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "common_structs.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -62,7 +62,7 @@ size_t getIndexWithLongitudinalOffset( if (longitudinal_offset > 0) { for (size_t i = *start_idx; i < points.size() - 1; ++i) { const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); sum_length += segment_length; if (sum_length >= longitudinal_offset) { const double back_length = sum_length - longitudinal_offset; @@ -79,7 +79,7 @@ size_t getIndexWithLongitudinalOffset( for (size_t i = *start_idx; 0 < i; --i) { const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)); + autoware_universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); sum_length += segment_length; if (sum_length >= -longitudinal_offset) { const double back_length = sum_length + longitudinal_offset; diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 2d3a96abfd81d..564c4d7cbe6a6 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -21,6 +21,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation @@ -35,7 +36,6 @@ std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 710308218d207..3c64f18cc0f05 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -16,12 +16,12 @@ #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -87,7 +87,7 @@ double calcDiffAngleAgainstTrajectory( const double target_yaw = tf2::getYaw(target_pose.orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware_universe_utils::normalizeRadian(target_yaw - traj_yaw); return diff_yaw; } @@ -130,7 +130,7 @@ TrajectoryPoint getExtendTrajectoryPoint( const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + extend_trajectory_point.pose = autoware_universe_utils::calcOffsetPose( goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; @@ -191,7 +191,7 @@ std::vector resampleTrajectoryPoints( return motion_utils::convertToTrajectoryPointArray(resampled_traj); } -geometry_msgs::msg::Point toGeomPoint(const tier4_autoware_utils::Point2d & point) +geometry_msgs::msg::Point toGeomPoint(const autoware_universe_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; geom_point.x = point.x(); @@ -278,75 +278,75 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( const std::vector & parameters) { // behavior determination - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", crossing_obstacle_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold", crossing_obstacle_traj_angle_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.stop.crossing_obstacle.collision_time_margin", collision_time_margin); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold", outside_obstacle_min_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold", ego_obstacle_overlap_time_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check", max_prediction_time_for_collision_check); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.stop_obstacle_hold_time_threshold", stop_obstacle_hold_time_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.prediction_resampling_time_interval", prediction_resampling_time_interval); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.prediction_resampling_time_horizon", prediction_resampling_time_horizon); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin_against_unknown", max_lat_margin_for_stop_against_unknown); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.lat_distance_threshold", yield_lat_distance_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles", max_lat_dist_between_obstacles); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold", stopped_obstacle_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time", max_obstacles_collision_time); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.slow_down.lat_hysteresis_margin", lat_hysteresis_margin_for_slow_down); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition", successive_num_to_entry_slow_down_condition); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", successive_num_to_exit_slow_down_condition); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", enable_to_consider_current_pose); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.time_to_convergence", time_to_convergence); } @@ -437,8 +437,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -457,17 +458,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( { planner_ptr_->onParam(parameters); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.enable_debug_info", enable_debug_info_); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.stop_on_curve.additional_safe_distance_margin", additional_safe_distance_margin_on_curve_); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.stop_on_curve.min_safe_distance_margin", min_safe_distance_margin_on_curve_); @@ -476,7 +477,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); behavior_determination_param_.onParam(parameters); @@ -571,7 +572,7 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); const auto nearest_pose = traj_points.at(nearest_idx).pose; const auto current_ego_pose_error = - tier4_autoware_utils::inverseTransformPose(current_ego_pose, nearest_pose); + autoware_universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); const double current_ego_lat_error = current_ego_pose_error.position.y; const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); double time_elapsed{0.0}; @@ -587,11 +588,11 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence; geometry_msgs::msg::Pose indexed_pose_err; indexed_pose_err.set__orientation( - tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + autoware_universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); indexed_pose_err.set__position( - tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + autoware_universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); current_poses.push_back( - tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + autoware_universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { time_elapsed += p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); @@ -605,23 +606,23 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { boost::geometry::append( idx_poly, - tier4_autoware_utils::toFootprint(pose, front_length, rear_length, vehicle_width) + autoware_universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) .outer()); boost::geometry::append( - idx_poly, - tier4_autoware_utils::fromMsg(tier4_autoware_utils::calcOffsetPose( - pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) - .position) - .to_2d()); + idx_poly, autoware_universe_utils::fromMsg( + autoware_universe_utils::calcOffsetPose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); boost::geometry::append( - idx_poly, tier4_autoware_utils::fromMsg( - tier4_autoware_utils::calcOffsetPose( + idx_poly, autoware_universe_utils::fromMsg( + autoware_universe_utils::calcOffsetPose( pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) .position) .to_2d()); } else { boost::geometry::append( - idx_poly, tier4_autoware_utils::toFootprint( + idx_poly, autoware_universe_utils::toFootprint( pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) .outer()); } @@ -650,7 +651,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::vector target_obstacles; for (const auto & predicted_object : objects.objects) { const auto & object_id = - tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); + autoware_universe_utils::toHexString(predicted_object.object_id).substr(0, 4); const auto & current_obstacle_pose = obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); @@ -1390,7 +1391,8 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&prev_closest_stop_obstacle](const PredictedObject & po) { - return tier4_autoware_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid; + return autoware_universe_utils::toHexString(po.object_id) == + prev_closest_stop_obstacle.uuid; }); // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { @@ -1524,10 +1526,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } } for (size_t i = 0; i < stop_collision_points.size(); ++i) { - auto collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto collision_point_marker = autoware_universe_utils::createDefaultMarker( "map", now(), "cruise_collision_points", i, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = stop_collision_points.at(i); debug_marker.markers.push_back(collision_point_marker); } @@ -1540,10 +1542,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision point - auto collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto collision_point_marker = autoware_universe_utils::createDefaultMarker( "map", now(), "stop_collision_points", 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; debug_marker.markers.push_back(collision_point_marker); } @@ -1557,16 +1559,16 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision points - auto front_collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto front_collision_point_marker = autoware_universe_utils::createDefaultMarker( "map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); front_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; - auto back_collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto back_collision_point_marker = autoware_universe_utils::createDefaultMarker( "map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); back_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; @@ -1583,10 +1585,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } { // footprint polygons - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", now(), "detection_polygons", 0, Marker::LINE_LIST, - tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + autoware_universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { @@ -1595,16 +1597,16 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); marker.points.push_back( - tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + autoware_universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + autoware_universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); } } debug_marker.markers.push_back(marker); } // slow down debug wall marker - tier4_autoware_utils::appendMarkerArray( + autoware_universe_utils::appendMarkerArray( debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); debug_marker_pub_->publish(debug_marker); diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 52daaa44936b4..032f3c0c536e2 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -15,6 +15,8 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" @@ -22,8 +24,6 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -450,7 +450,7 @@ std::optional OptimizationBasedPlanner::getSBoundaries( const auto markers = motion_utils::createSlowDownVirtualWallMarker( marker_pose.value(), "obstacle to follow", current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &wall_msg); + autoware_universe_utils::appendMarkerArray(markers, &wall_msg); // publish rviz marker debug_wall_marker_pub_->publish(wall_msg); @@ -627,7 +627,7 @@ geometry_msgs::msg::Pose OptimizationBasedPlanner::transformBaseLink2Center( geometry_msgs::msg::Pose center_pose; center_pose.position = - tier4_autoware_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); + autoware_universe_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); center_pose.orientation = pose_base_link.orientation; return center_pose; diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 06ac8f655a122..01cb454aa4692 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -15,11 +15,11 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "interpolation/spline_interpolation.hpp" #include "motion_utils/marker/marker_helper.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" @@ -317,8 +317,9 @@ std::vector PIDBasedPlanner::planCruise( stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0); // NOTE: use a different color from slow down one to visualize cruise and slow down // separately. - markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + markers.markers.front().color = + autoware_universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); + autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); @@ -345,7 +346,7 @@ std::vector PIDBasedPlanner::planCruise( // delete marker const auto markers = motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); return stop_traj_points; } @@ -598,7 +599,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( double sum_dist = 0.0; for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { if (i != ego_seg_idx) { - sum_dist += tier4_autoware_utils::calcDistance2d( + sum_dist += autoware_universe_utils::calcDistance2d( acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); } @@ -621,7 +622,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( void PIDBasedPlanner::updateCruiseParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); { // velocity limit based planner @@ -631,26 +632,26 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki = p.pid_vel_controller->getKi(); double kd = p.pid_vel_controller->getKd(); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kp", kp); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.ki", ki); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kd", kd); p.pid_vel_controller->updateParam(kp, ki, kd); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", p.output_ratio_during_accel); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", p.disable_target_acceleration); @@ -664,11 +665,11 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_acc = p.pid_acc_controller->getKi(); double kd_acc = p.pid_acc_controller->getKd(); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); @@ -677,32 +678,32 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_jerk = p.pid_jerk_controller->getKi(); double kd_jerk = p.pid_jerk_controller->getKd(); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", p.output_acc_ratio_during_accel); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", p.output_jerk_ratio_during_accel); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); } // min_cruise_target_vel - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "cruise.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index f861928dae226..56cf204e64a51 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -14,13 +14,13 @@ #include "autoware/obstacle_cruise_planner/planner_interface.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "motion_utils/distance/distance.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "signal_processing/lowpass_filter_1d.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" #include #include @@ -223,9 +223,9 @@ std::vector resampleTrajectoryPoints( return motion_utils::convertToTrajectoryPointArray(resampled_traj); } -tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +autoware_universe_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) { - return tier4_autoware_utils::Point2d{p.x, p.y}; + return autoware_universe_utils::Point2d{p.x, p.y}; } } // namespace @@ -249,7 +249,7 @@ std::vector PlannerInterface::generateStopTrajectory( // delete marker const auto markers = motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; @@ -356,7 +356,7 @@ std::vector PlannerInterface::generateStopTrajectory( // delete marker const auto markers = motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; @@ -392,7 +392,7 @@ std::vector PlannerInterface::generateStopTrajectory( const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // Publish Stop Reason @@ -459,7 +459,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { break; } - sum_short_traj_length += tier4_autoware_utils::calcDistance2d( + sum_short_traj_length += autoware_universe_utils::calcDistance2d( planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); } std::reverse(short_traj_points.begin(), short_traj_points.end()); @@ -470,15 +470,15 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate collision index between straight line from ego pose and object const auto calculate_distance_from_straight_ego_path = [&](const auto & ego_pose, const auto & object_polygon) { - const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( + const auto forward_ego_pose = autoware_universe_utils::calcOffsetPose( ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); - const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ + const auto ego_straight_segment = autoware_universe_utils::Segment2d{ convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; return boost::geometry::distance(ego_straight_segment, object_polygon); }; const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); const auto object_polygon = - tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + autoware_universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); const auto collision_idx = [&]() -> std::optional { for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { const double dist_to_obj = calculate_distance_from_straight_ego_path( @@ -498,7 +498,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate margin from obstacle const double partial_segment_length = [&]() { - const double collision_segment_length = tier4_autoware_utils::calcDistance2d( + const double collision_segment_length = autoware_universe_utils::calcDistance2d( resampled_short_traj_points.at(*collision_idx - 1), resampled_short_traj_points.at(*collision_idx)); const double prev_dist = calculate_distance_from_straight_ego_path( @@ -649,7 +649,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } // add debug virtual wall @@ -657,14 +657,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray( + autoware_universe_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray( + autoware_universe_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp index 8580e10b19652..4ae0b8ba5f87c 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" namespace { @@ -51,10 +51,10 @@ std::optional>> getCollisionIndex( const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object_pose, object_shape); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = - tier4_autoware_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + autoware_universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); if (approximated_dist > max_dist) { continue; } @@ -105,14 +105,14 @@ std::optional> getCollisionPoint( const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m : vehicle_info.min_longitudinal_offset_m; - const auto bumper_pose = tier4_autoware_utils::calcOffsetPose( + const auto bumper_pose = autoware_universe_utils::calcOffsetPose( traj_points.at(collision_info->first).pose, x_diff_to_bumper, 0.0, 0.0); std::optional max_collision_length = std::nullopt; std::optional max_collision_point = std::nullopt; for (const auto & poly_vertex : collision_info->second) { const double dist_from_bumper = - std::abs(tier4_autoware_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + std::abs(autoware_universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { max_collision_length = dist_from_bumper; diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index d90ca80ced08c..b95fb6e5d29f9 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -14,8 +14,8 @@ #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace obstacle_cruise_utils { @@ -59,10 +59,10 @@ visualization_msgs::msg::Marker getObjectMarker( { const auto current_time = rclcpp::Clock().now(); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(2.0, 2.0, 2.0), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.8)); + autoware_universe_utils::createMarkerScale(2.0, 2.0, 2.0), + autoware_universe_utils::createMarkerColor(r, g, b, 0.8)); marker.pose = obj_pose; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 67a58fabaa0f3..df9dec3cf3b75 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -16,9 +16,9 @@ #define AUTOWARE__PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ #include "autoware/path_optimizer/type_alias.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -89,7 +89,7 @@ struct TimeKeeper double accumulated_time{0.0}; - tier4_autoware_utils::StopWatch< + autoware_universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -122,7 +122,7 @@ struct TrajectoryParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; // common updateParam( @@ -145,7 +145,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } 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 4022b6b24f02d..487320330f953 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/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.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 #include 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 f2354e9c4c3c3..cece0142bc29a 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/universe_utils/geometry/geometry.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 #include @@ -93,9 +93,9 @@ struct ReferencePoint geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const { - auto pose_with_deviation = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); + auto pose_with_deviation = autoware_universe_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - tier4_autoware_utils::createQuaternionFromYaw(getYaw() + yaw_dev); + autoware_universe_utils::createQuaternionFromYaw(getYaw() + yaw_dev); return pose_with_deviation; } }; 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 100c7b6fb1755..7f8755fd043d9 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -19,13 +19,13 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" #include "autoware/path_optimizer/replan_checker.hpp" #include "autoware/path_optimizer/type_alias.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.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 +#include #include #include @@ -90,7 +90,7 @@ class PathOptimizer : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ this, "~/input/odometry"}; // debug publisher @@ -137,9 +137,9 @@ class PathOptimizer : public rclcpp::Node private: double vehicle_stop_margin_outside_drivable_area_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::path_optimizer 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 bfca87604551b..9737137ad8183 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 @@ -36,14 +36,14 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils namespace autoware::path_optimizer { @@ -52,8 +52,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = tier4_autoware_utils::getPoint(t1); - const auto p2 = tier4_autoware_utils::getPoint(t2); + const auto p1 = autoware_universe_utils::getPoint(t1); + const auto p2 = autoware_universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index 6c38a3e8e0c05..ef22f668b6a09 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -35,7 +35,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); @@ -45,7 +45,7 @@ geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint template <> double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p); -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils namespace autoware::path_optimizer { @@ -68,7 +68,7 @@ std::optional getPointIndexAfter( // search forward if (sum_length < min_offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (min_offset < sum_length) { output_idx = i - 1; } @@ -82,7 +82,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < min_offset) { output_idx = i - 1; } @@ -98,7 +98,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.pose = autoware_universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -109,8 +109,9 @@ template <> inline TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(ref_point); - traj_point.longitudinal_velocity_mps = tier4_autoware_utils::getLongitudinalVelocity(ref_point); + traj_point.pose = autoware_universe_utils::getPose(ref_point); + traj_point.longitudinal_velocity_mps = + autoware_universe_utils::getLongitudinalVelocity(ref_point); return traj_point; } @@ -172,7 +173,7 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware_universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); @@ -186,7 +187,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware_universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index c158815999398..39d967df8f077 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_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation @@ -26,7 +27,6 @@ rclcpp_components std_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index 8068f705799ab..eea6ada555044 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -20,10 +20,10 @@ namespace autoware::path_optimizer { -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; namespace { @@ -53,16 +53,16 @@ MarkerArray getFootprintsMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) + autoware_universe_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) + autoware_universe_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) + autoware_universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) + autoware_universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position); marker.points.push_back(marker.points.front()); @@ -110,7 +110,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware_universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); lb_marker.points.push_back(lb); @@ -132,7 +132,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware_universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); ub_marker.points.push_back(ub); @@ -167,11 +167,11 @@ MarkerArray getBoundsLineMarkerArray( const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware_universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware_universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } marker_array.markers.push_back(ub_marker); @@ -205,22 +205,22 @@ MarkerArray getVehicleCircleLinesMarkerArray( // apply lateral and yaw deviation auto pose_with_deviation = - tier4_autoware_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); + autoware_universe_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); + autoware_universe_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); for (const double d : vehicle_circle_longitudinal_offsets) { // apply longitudinal offset - auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); + auto base_pose = autoware_universe_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); + autoware_universe_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const auto ub = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; + autoware_universe_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; + autoware_universe_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -247,7 +247,7 @@ MarkerArray getCurrentVehicleCirclesMarkerArray( "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); + marker.pose = autoware_universe_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -289,7 +289,7 @@ MarkerArray getVehicleCirclesMarkerArray( "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); + marker.pose = autoware_universe_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -356,13 +356,15 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); + autoware_universe_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0).position); + autoware_universe_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0) + .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0).position); + autoware_universe_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0) + .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); + autoware_universe_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 4696802bfca0b..38d1dc1309cf3 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -16,12 +16,12 @@ #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -125,8 +125,8 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { const double base_theta = tf2::getYaw(pose.orientation); - const double target_theta = tier4_autoware_utils::calcAzimuthAngle(pose.position, target_pos); - const double diff_theta = tier4_autoware_utils::normalizeRadian(target_theta - base_theta); + const double target_theta = autoware_universe_utils::calcAzimuthAngle(pose.position, target_pos); + const double diff_theta = autoware_universe_utils::normalizeRadian(target_theta - base_theta); return diff_theta > 0; } @@ -141,18 +141,18 @@ double calcLateralDistToBounds( const double max_lat_offset = is_left_bound ? max_lat_offset_for_left : -max_lat_offset_for_left; const double min_lat_offset = is_left_bound ? min_lat_offset_for_left : -min_lat_offset_for_left; const auto max_lat_offset_point = - tier4_autoware_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; + autoware_universe_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; const auto min_lat_offset_point = - tier4_autoware_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; + autoware_universe_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; double closest_dist_to_bound = max_lat_offset; for (size_t i = 0; i < bound.size() - 1; ++i) { - const auto intersect_point = tier4_autoware_utils::intersect( + const auto intersect_point = autoware_universe_utils::intersect( min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); if (intersect_point) { const bool is_point_left = isLeft(pose, *intersect_point); const double dist_to_bound = - tier4_autoware_utils::calcDistance2d(pose.position, *intersect_point) * + autoware_universe_utils::calcDistance2d(pose.position, *intersect_point) * (is_point_left ? 1.0 : -1.0); // the bound which is closest to the centerline will be chosen @@ -283,7 +283,7 @@ MPTOptimizer::MPTParam::MPTParam( void MPTOptimizer::MPTParam::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; { // option updateParam(parameters, "mpt.option.steer_limit_constraint", steer_limit_constraint); @@ -623,7 +623,7 @@ void MPTOptimizer::updateOrientation( const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); for (size_t i = 0; i < ref_points.size(); ++i) { ref_points.at(i).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i)); + autoware_universe_utils::createQuaternionFromYaw(yaw_vec.at(i)); } } @@ -691,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points ref_points.at(i).delta_arc_length = (i == ref_points.size() - 1) ? 0.0 - : tier4_autoware_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); + : autoware_universe_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); } } @@ -704,14 +704,14 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c const auto front_wheel_pos = trajectory_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); - const bool are_too_close_points = - tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; - const auto front_wheel_yaw = - are_too_close_points - ? ref_points.at(i).getYaw() - : tier4_autoware_utils::calcAzimuthAngle(ref_points.at(i).pose.position, front_wheel_pos); + const bool are_too_close_points = autoware_universe_utils::calcDistance2d( + front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; + const auto front_wheel_yaw = are_too_close_points + ? ref_points.at(i).getYaw() + : autoware_universe_utils::calcAzimuthAngle( + ref_points.at(i).pose.position, front_wheel_pos); ref_points.at(i).alpha = - tier4_autoware_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); + autoware_universe_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); } { // avoidance @@ -771,10 +771,10 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c if (prev_ref_points_ptr_ && !prev_ref_points_ptr_->empty()) { for (int i = 0; i < static_cast(ref_points.size()); ++i) { const size_t prev_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.at(i)), + *prev_ref_points_ptr_, autoware_universe_utils::getPose(ref_points.at(i)), ego_nearest_param_); - const double dist_to_prev = tier4_autoware_utils::calcDistance2d( + const double dist_to_prev = autoware_universe_utils::calcDistance2d( ref_points.at(i), prev_ref_points_ptr_->at(prev_idx)); if (max_dist_threshold < dist_to_prev) { continue; @@ -1081,7 +1081,8 @@ void MPTOptimizer::avoidSuddenSteering( return; } const size_t prev_ego_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.front()), ego_nearest_param_); + *prev_ref_points_ptr_, autoware_universe_utils::getPose(ref_points.front()), + ego_nearest_param_); const double max_bound_fixing_length = ego_vel * mpt_param_.max_bound_fixing_time; const int max_bound_fixing_idx = @@ -1128,11 +1129,11 @@ void MPTOptimizer::updateVehicleBounds( collision_check_pose.position.y - ref_point.pose.position.y, collision_check_pose.position.x - ref_point.pose.position.x); const double offset_y = - -tier4_autoware_utils::calcDistance2d(ref_point, collision_check_pose) * + -autoware_universe_utils::calcDistance2d(ref_point, collision_check_pose) * std::sin(tmp_yaw - collision_check_yaw); const auto vehicle_bounds_pose = - tier4_autoware_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); + autoware_universe_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); // interpolate bounds const auto bounds = [&]() { diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 7ae169f35d8b5..c6c6511c91133 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -75,7 +75,7 @@ std::vector calcSegmentLengthVector(const std::vector & std::vector segment_length_vector; for (size_t i = 0; i < points.size() - 1; ++i) { const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + autoware_universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); segment_length_vector.push_back(segment_length); } return segment_length_vector; @@ -150,14 +150,15 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) set_param_res_ = this->add_on_set_parameters_callback( std::bind(&PathOptimizer::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; // parameters for option updateParam( @@ -562,7 +563,7 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); if (!enable_outside_drivable_area_stop_) { virtual_wall_marker.markers.front().color = - tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.5); + autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.5); } virtual_wall_pub_->publish(virtual_wall_marker); diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index 706c55679196e..dda0a9142d3dc 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -15,9 +15,9 @@ #include "autoware/path_optimizer/replan_checker.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -39,7 +39,7 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam( parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); @@ -78,7 +78,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // ego pose is lost or new ego pose is designated in simulation const double delta_dist = - tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + autoware_universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -205,7 +205,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware_universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 9c87117e9cf81..4b51a9cee5282 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -18,7 +18,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include +#include #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -39,10 +39,10 @@ namespace autoware::path_optimizer { namespace bg = boost::geometry; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; namespace { @@ -134,13 +134,13 @@ bool isOutsideDrivableAreaFromRectangleFootprint( // calculate footprint corner points const auto top_left_pos = - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; + autoware_universe_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; + autoware_universe_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; const auto bottom_right_pos = - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; + autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; const auto bottom_left_pos = - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; + autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; if (use_footprint_polygon_for_outside_drivable_area_check) { // calculate footprint polygon diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 08fa7542b490b..fe377b60f5030 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -33,7 +33,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p) @@ -52,7 +52,7 @@ double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & { return p.longitudinal_velocity_mps; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils namespace autoware::path_optimizer { @@ -109,11 +109,11 @@ void compensateLastPose( const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; const double dist = - tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + autoware_universe_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); const double norm_diff_yaw = [&]() { const double diff_yaw = tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return tier4_autoware_utils::normalizeRadian(diff_yaw); + return autoware_universe_utils::normalizeRadian(diff_yaw); }(); if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { traj_points.push_back(convertToTrajectoryPoint(last_path_point)); @@ -175,7 +175,7 @@ std::vector resampleReferencePoints( base_keys.push_back(0.0); } else { const double delta_arc_length = - tier4_autoware_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); + autoware_universe_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); base_keys.push_back(base_keys.back() + delta_arc_length); } @@ -187,7 +187,7 @@ std::vector resampleReferencePoints( if (i == 0) { query_keys.push_back(0.0); } else { - const double delta_arc_length = tier4_autoware_utils::calcDistance2d( + const double delta_arc_length = autoware_universe_utils::calcDistance2d( resampled_ref_points.at(i), resampled_ref_points.at(i - 1)); const double key = query_keys.back() + delta_arc_length; if (base_keys.back() < key) { diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp index c845a1c8e7454..e26d13b94ecb0 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp @@ -16,9 +16,9 @@ #define AUTOWARE__PATH_SMOOTHER__COMMON_STRUCTS_HPP_ #include "autoware/path_smoother/type_alias.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -83,7 +83,7 @@ struct TimeKeeper double accumulated_time{0.0}; - tier4_autoware_utils::StopWatch< + autoware_universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -100,7 +100,7 @@ struct CommonParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; // common updateParam( @@ -123,7 +123,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp index 89200fddd57e7..defaae9977b7d 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp @@ -19,12 +19,12 @@ #include "autoware/path_smoother/elastic_band.hpp" #include "autoware/path_smoother/replan_checker.hpp" #include "autoware/path_smoother/type_alias.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.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 +#include #include #include @@ -82,7 +82,8 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; + autoware_universe_utils::InterProcessPollingSubscriber odom_sub_{ + this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -112,9 +113,9 @@ class ElasticBandSmoother : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::path_smoother diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp index ad6dbfb5cab74..df9dbdf7f7e49 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE__PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ -#include +#include namespace autoware::path_smoother { @@ -24,8 +24,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = tier4_autoware_utils::getPoint(t1); - const auto p2 = tier4_autoware_utils::getPoint(t2); + const auto p1 = autoware_universe_utils::getPoint(t1); + const auto p2 = autoware_universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; return (std::abs(p1.x - p2.x) <= epsilon && std::abs(p1.y - p2.y) <= epsilon); diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index 23a46cc842f75..6c76593b78328 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -56,7 +56,7 @@ std::optional getPointIndexAfter( // search forward if (sum_length < min_offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (min_offset < sum_length) { output_idx = i - 1; } @@ -70,7 +70,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < min_offset) { output_idx = i - 1; } @@ -86,7 +86,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.pose = autoware_universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -128,7 +128,7 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware_universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); @@ -142,7 +142,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware_universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 445f8a0e6e736..62a59a79c2bc9 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -16,6 +16,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils geometry_msgs interpolation motion_utils @@ -25,7 +26,6 @@ rclcpp_components std_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index cb9f33281f60f..151a0252945f2 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -125,7 +125,7 @@ EBPathSmoother::EBParam::EBParam(rclcpp::Node * node) void EBPathSmoother::EBParam::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; { // option updateParam(parameters, "elastic_band.option.enable_warm_start", enable_warm_start); @@ -443,7 +443,7 @@ std::optional> EBPathSmoother::convertOptimizedPoin auto eb_traj_point = traj_points.at(i); eb_traj_point.pose = - tier4_autoware_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); eb_traj_points.push_back(eb_traj_point); } diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index 11de66ef541fe..e5f30784a97c5 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -105,14 +105,15 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; // parameters for ego nearest search ego_nearest_param_.onParam(parameters); diff --git a/planning/autoware_path_smoother/src/replan_checker.cpp b/planning/autoware_path_smoother/src/replan_checker.cpp index 719d7f1376c25..d9f7f4fb6ca18 100644 --- a/planning/autoware_path_smoother/src/replan_checker.cpp +++ b/planning/autoware_path_smoother/src/replan_checker.cpp @@ -15,9 +15,9 @@ #include "autoware/path_smoother/replan_checker.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -40,7 +40,7 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "replan.enable", enable_); updateParam( @@ -80,7 +80,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // ego pose is lost or new ego pose is designated in simulation const double delta_dist = - tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + autoware_universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -201,7 +201,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware_universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp index 0f107a97d09f0..dfaefd7c882d1 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -57,8 +57,8 @@ Pose createPoseFromLaneID(const lanelet::Id & lane_id) // calculate middle pose geometry_msgs::msg::Pose middle_pose; middle_pose.position = middle_pos; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware_universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); return middle_pose; } diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index 6ccebfc2f56fe..0dac02e92e617 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -19,6 +19,7 @@ autoware_planning_msgs autoware_route_handler autoware_test_utils + autoware_universe_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -30,7 +31,6 @@ tf2_msgs tf2_ros tier4_api_msgs - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 715dc6277f419..5002b8fa08fc1 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -14,10 +14,10 @@ #include "motion_utils/trajectory/conversion.hpp" +#include #include #include #include -#include namespace autoware::planning_test_manager { diff --git a/planning/autoware_planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml index be44523fa9852..c6d7c08ca2a08 100644 --- a/planning/autoware_planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -15,10 +15,10 @@ ament_cmake_auto autoware_planning_msgs + autoware_universe_utils motion_utils rclcpp rclcpp_components - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index d0ea18e079057..b9e260205632e 100644 --- a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_topic_converter/path_to_trajectory.hpp" +#include #include -#include namespace autoware::planning_topic_converter { @@ -24,7 +24,7 @@ namespace TrajectoryPoint convertToTrajectoryPoint(const PathPoint & point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.pose = autoware_universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp index df25332328228..1d47aefaa31e4 100644 --- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp @@ -16,15 +16,15 @@ #define AUTOWARE__PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ #include "autoware/planning_validator/debug_marker.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/stop_watch.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/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include #include #include -#include #include #include @@ -39,10 +39,10 @@ namespace autoware::planning_validator using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_planning_validator::msg::PlanningValidatorStatus; +using autoware_universe_utils::StopWatch; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; -using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; struct ValidationParams @@ -103,7 +103,7 @@ class PlanningValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; @@ -137,9 +137,9 @@ class PlanningValidator : public rclcpp::Node std::shared_ptr debug_pose_publisher_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; StopWatch stop_watch_; }; diff --git a/planning/autoware_planning_validator/package.xml b/planning/autoware_planning_validator/package.xml index 541396b52b02b..3bd1574b5a38e 100644 --- a/planning/autoware_planning_validator/package.xml +++ b/planning/autoware_planning_validator/package.xml @@ -17,6 +17,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs @@ -24,7 +25,6 @@ nav_msgs rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/planning/autoware_planning_validator/src/debug_marker.cpp b/planning/autoware_planning_validator/src/debug_marker.cpp index 3dc1012ee4a22..0defa72684c92 100644 --- a/planning/autoware_planning_validator/src/debug_marker.cpp +++ b/planning/autoware_planning_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_validator/debug_marker.hpp" +#include #include -#include #include #include @@ -48,7 +48,7 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) { - using tier4_autoware_utils::createMarkerColor; + using autoware_universe_utils::createMarkerColor; // append arrow marker std_msgs::msg::ColorRGBA color; @@ -64,9 +64,9 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( { color = createMarkerColor(0.0, 0.0, 1.0, 0.999); } - Marker marker = tier4_autoware_utils::createDefaultMarker( + Marker marker = autoware_universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); + autoware_universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; @@ -76,10 +76,10 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushWarningMsg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware_universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware_universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -91,7 +91,7 @@ void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs: const auto now = node_->get_clock()->now(); const auto stop_wall_marker = motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); - tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + autoware_universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } void PlanningValidatorDebugMarkerPublisher::publish() diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp index e140f35d0e9f4..1e095e2fa424e 100644 --- a/planning/autoware_planning_validator/src/planning_validator.cpp +++ b/planning/autoware_planning_validator/src/planning_validator.cpp @@ -16,8 +16,8 @@ #include "autoware/planning_validator/utils.hpp" +#include #include -#include #include #include @@ -44,8 +44,9 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void PlanningValidator::setupParameters() @@ -468,8 +469,8 @@ bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajector const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); - validation_status_.distance_deviation = - tier4_autoware_utils::calcDistance2d(trajectory.points.at(idx), current_kinematics_->pose.pose); + validation_status_.distance_deviation = autoware_universe_utils::calcDistance2d( + trajectory.points.at(idx), current_kinematics_->pose.pose); if (validation_status_.distance_deviation > validation_params_.distance_deviation_threshold) { return false; @@ -500,7 +501,7 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory // for last, need to remove distance for the last segment. if (is_last) { const auto size = trajectory.points.size(); - long_offset -= tier4_autoware_utils::calcDistance2d( + long_offset -= autoware_universe_utils::calcDistance2d( trajectory.points.at(size - 1), trajectory.points.at(size - 2)); } diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp index d94598aa5951d..1f64e7c4ad078 100644 --- a/planning/autoware_planning_validator/src/utils.cpp +++ b/planning/autoware_planning_validator/src/utils.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_validator/utils.hpp" +#include #include -#include #include #include @@ -24,9 +24,9 @@ namespace autoware::planning_validator { -using tier4_autoware_utils::calcCurvature; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getPoint; +using autoware_universe_utils::calcCurvature; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::getPoint; namespace { @@ -138,7 +138,7 @@ void calcCurvature( const auto p2 = getPoint(trajectory.points.at(i)); const auto p3 = getPoint(trajectory.points.at(next_idx)); try { - curvature_arr.at(i) = tier4_autoware_utils::calcCurvature(p1, p2, p3); + curvature_arr.at(i) = autoware_universe_utils::calcCurvature(p1, p2, p3); } catch (...) { curvature_arr.at(i) = 0.0; // maybe distance is too close } @@ -236,11 +236,12 @@ std::pair calcMaxRelativeAngles(const Trajectory & trajectory) const auto & p2 = trajectory.points.at(i + 1).pose.position; const auto & p3 = trajectory.points.at(i + 2).pose.position; - const auto angle_a = tier4_autoware_utils::calcAzimuthAngle(p1, p2); - const auto angle_b = tier4_autoware_utils::calcAzimuthAngle(p2, p3); + const auto angle_a = autoware_universe_utils::calcAzimuthAngle(p1, p2); + const auto angle_b = autoware_universe_utils::calcAzimuthAngle(p2, p3); // convert relative angle to [-pi ~ pi] - const auto relative_angle = std::abs(tier4_autoware_utils::normalizeRadian(angle_b - angle_a)); + const auto relative_angle = + std::abs(autoware_universe_utils::normalizeRadian(angle_b - angle_a)); takeBigger(max_relative_angles, max_index, std::abs(relative_angle), i); } diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp index de7d8293286d7..b4985700fdcb6 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp @@ -14,14 +14,14 @@ #include "test_planning_validator_helper.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "test_parameter.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::createQuaternionFromYaw; +using autoware_universe_utils::createQuaternionFromYaw; Trajectory generateTrajectoryWithConstantAcceleration( const double interval_distance, const double speed, const double yaw, const size_t size, diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 0e5ae5aa63c3e..494c0a58e1e02 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -15,6 +15,7 @@ autoware_planning_msgs autoware_route_handler autoware_test_utils + autoware_universe_utils geometry_msgs lanelet2_extension motion_utils @@ -22,7 +23,6 @@ rclcpp rclcpp_components std_msgs - tier4_autoware_utils tier4_planning_msgs ament_cmake_ros diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 327bd0ff3b582..4c1eb1d1c4ef6 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -14,12 +14,12 @@ #include "remaining_distance_time_calculator_node.hpp" +#include #include #include #include #include #include -#include #include @@ -143,8 +143,8 @@ void RemainingDistanceTimeCalculatorNode::calculate_remaining_distance() for (auto & llt : remaining_shortest_path) { if (remaining_shortest_path.size() == 1) { - remaining_distance_ += - tier4_autoware_utils::calcDistance2d(current_vehicle_pose_.position, goal_pose_.position); + remaining_distance_ += autoware_universe_utils::calcDistance2d( + current_vehicle_pose_.position, goal_pose_.position); break; } diff --git a/planning/autoware_route_handler/package.xml b/planning/autoware_route_handler/package.xml index 64cdaf596b52d..f7d10a519ed4e 100644 --- a/planning/autoware_route_handler/package.xml +++ b/planning/autoware_route_handler/package.xml @@ -26,12 +26,12 @@ autoware_map_msgs autoware_planning_msgs autoware_test_utils + autoware_universe_utils geometry_msgs lanelet2_extension rclcpp rclcpp_components tf2_ros - tier4_autoware_utils yaml-cpp diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 12adab590e9bd..cffac44b6947e 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -14,13 +14,13 @@ #include "autoware/route_handler/route_handler.hpp" +#include #include #include #include #include #include #include -#include #include #include @@ -110,7 +110,7 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) constexpr double min_dist = 0.001; if ( - tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < + autoware_universe_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < min_dist) { filtered_path.points.back().lane_ids.push_back(pt.lane_ids.front()); filtered_path.points.back().point.longitudinal_velocity_mps = std::min( @@ -1725,14 +1725,14 @@ PathWithLaneId RouteHandler::getCenterLinePath( double angle{0.0}; const auto & pts = reference_path.points; if (i + 1 < reference_path.points.size()) { - angle = tier4_autoware_utils::calcAzimuthAngle( + angle = autoware_universe_utils::calcAzimuthAngle( pts.at(i).point.pose.position, pts.at(i + 1).point.pose.position); } else if (i != 0) { - angle = tier4_autoware_utils::calcAzimuthAngle( + angle = autoware_universe_utils::calcAzimuthAngle( pts.at(i - 1).point.pose.position, pts.at(i).point.pose.position); } reference_path.points.at(i).point.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(angle); + autoware_universe_utils::createQuaternionFromYaw(angle); } return reference_path; diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp index eb39b37a81fc3..056d46056f452 100644 --- a/planning/autoware_route_handler/test/test_route_handler.cpp +++ b/planning/autoware_route_handler/test/test_route_handler.cpp @@ -14,7 +14,7 @@ #include "test_route_handler.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -53,10 +53,10 @@ TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) ASSERT_FALSE(route_handler_->isHandlerReady()); geometry_msgs::msg::Pose start_pose, goal_pose; - start_pose.position = tier4_autoware_utils::createPoint(3728.870361, 73739.281250, 0); - start_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, -0.513117, 0.858319); - goal_pose.position = tier4_autoware_utils::createPoint(3729.961182, 73727.328125, 0); - goal_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, 0.234831, 0.972036); + start_pose.position = autoware_universe_utils::createPoint(3728.870361, 73739.281250, 0); + start_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, -0.513117, 0.858319); + goal_pose.position = autoware_universe_utils::createPoint(3729.961182, 73727.328125, 0); + goal_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, 0.234831, 0.972036); lanelet::ConstLanelets path_lanelets; ASSERT_TRUE( @@ -86,16 +86,16 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) geometry_msgs::msg::Pose reference_pose, search_pose; lanelet::ConstLanelet reference_lanelet; - reference_pose.position = tier4_autoware_utils::createPoint(3730.88, 73735.3, 0); - reference_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, -0.504626, 0.863338); + reference_pose.position = autoware_universe_utils::createPoint(3730.88, 73735.3, 0); + reference_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, -0.504626, 0.863338); const auto found_reference_lanelet = route_handler_->getClosestLaneletWithinRoute(reference_pose, &reference_lanelet); ASSERT_TRUE(found_reference_lanelet); ASSERT_EQ(reference_lanelet.id(), 168); lanelet::ConstLanelet closest_lanelet; - search_pose.position = tier4_autoware_utils::createPoint(3736.89, 73730.8, 0); - search_pose.orientation = tier4_autoware_utils::createQuaternion(0, 0, 0.223244, 0.974763); + search_pose.position = autoware_universe_utils::createPoint(3736.89, 73730.8, 0); + search_pose.orientation = autoware_universe_utils::createQuaternion(0, 0, 0.223244, 0.974763); bool found_lanelet = route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lanelet); ASSERT_TRUE(found_lanelet); ASSERT_EQ(closest_lanelet.id(), 345); @@ -112,56 +112,56 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) // Pose search_pose; -// search_pose.position = tier4_autoware_utils::createPoint(-1.0, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware_universe_utils::createPoint(-1.0, 1.75, 0); +// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained7 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained7); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(-0.5, 1.75, 0); +// search_pose.position = autoware_universe_utils::createPoint(-0.5, 1.75, 0); // const auto closest_lane_obtained = -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(-.01, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware_universe_utils::createPoint(-.01, 1.75, 0); +// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained3 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained3); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(0.0, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware_universe_utils::createPoint(0.0, 1.75, 0); +// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained1 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained1); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(0.01, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware_universe_utils::createPoint(0.01, 1.75, 0); +// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained2 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained2); // ASSERT_EQ(closest_lane.id(), 4424); -// search_pose.position = tier4_autoware_utils::createPoint(0.5, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware_universe_utils::createPoint(0.5, 1.75, 0); +// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained4 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained4); // ASSERT_EQ(closest_lane.id(), 4424); -// search_pose.position = tier4_autoware_utils::createPoint(1.0, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware_universe_utils::createPoint(1.0, 1.75, 0); +// search_pose.orientation = autoware_universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained5 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 51ec514b9f284..e008a3d647436 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ #define AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ +#include #include -#include #include #include @@ -35,7 +35,7 @@ #include #endif #include -#include +#include #include #include @@ -73,12 +73,12 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_route_; - tier4_autoware_utils::InterProcessPollingSubscriber::SharedPtr + autoware_universe_utils::InterProcessPollingSubscriber::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_lane_driving_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_trajectory_; - tier4_autoware_utils::InterProcessPollingSubscriber::SharedPtr + autoware_universe_utils::InterProcessPollingSubscriber::SharedPtr sub_parking_state_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; @@ -93,7 +93,7 @@ class ScenarioSelectorNode : public rclcpp::Node std::deque twist_buffer_; std::shared_ptr route_handler_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // Parameters double update_rate_; diff --git a/planning/autoware_scenario_selector/package.xml b/planning/autoware_scenario_selector/package.xml index 03a0e92b5a33f..4a3c0c5c99eaf 100644 --- a/planning/autoware_scenario_selector/package.xml +++ b/planning/autoware_scenario_selector/package.xml @@ -20,6 +20,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_universe_utils lanelet2_extension nav_msgs rclcpp @@ -27,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs ros2cli diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 08f85ea4245ee..d1516c68050c1 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -381,7 +381,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index e6b812ff9b388..684d146c141ba 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -23,6 +23,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geography_utils geometry_msgs @@ -35,7 +36,6 @@ osqp_interface rclcpp rclcpp_components - tier4_autoware_utils tier4_map_msgs python3-flask-cors diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 447773d7a6535..d73573833d1fa 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -73,10 +73,10 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) centerline_traj_points.at(i - 1).pose.orientation; } } else { - const double yaw_angle = tier4_autoware_utils::calcAzimuthAngle( + const double yaw_angle = autoware_universe_utils::calcAzimuthAngle( centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); centerline_traj_points.at(i).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_angle); + autoware_universe_utils::createQuaternionFromYaw(yaw_angle); } } diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 8b403bbfe5c73..40dc817d2d671 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -16,10 +16,10 @@ #include "autoware/path_optimizer/node.hpp" #include "autoware/path_smoother/elastic_band_smoother.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "static_centerline_generator_node.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" #include "utils.hpp" #include @@ -82,13 +82,13 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( // get ego nearest search parameters and resample interval in behavior_path_planner const double ego_nearest_dist_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + autoware_universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); const double ego_nearest_yaw_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + autoware_universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); const double behavior_path_interval = - tier4_autoware_utils::getOrDeclareParameter(node, "output_path_interval"); + autoware_universe_utils::getOrDeclareParameter(node, "output_path_interval"); const double behavior_vel_interval = - tier4_autoware_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); + autoware_universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { @@ -165,7 +165,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // connect the previously and currently optimized trajectory points for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware_universe_utils::calcDistance2d( whole_optimized_traj_points.at(j), optimized_traj_points.front()); if (dist < 0.5) { const std::vector extracted_whole_optimized_traj_points{ 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 f5771b9b0b4e8..b67e7cbc0a680 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 @@ -14,6 +14,8 @@ #include "static_centerline_generator_node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "centerline_source/bag_ego_trajectory_based_centerline.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" @@ -24,15 +26,13 @@ #include "map_projection_loader/map_projection_loader.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" #include "type_alias.hpp" #include "utils.hpp" #include +#include #include #include -#include #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" @@ -88,10 +88,14 @@ LinearRing2d create_vehicle_footprint( const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); std::vector geom_points; - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); + geom_points.push_back( + autoware_universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); + geom_points.push_back( + autoware_universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); + geom_points.push_back( + autoware_universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); + geom_points.push_back( + autoware_universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); LinearRing2d footprint; for (const auto & geom_point : geom_points) { @@ -113,7 +117,7 @@ geometry_msgs::msg::Pose get_text_pose( const double x_front = i.front_overhang_m + i.wheel_base_m; const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 1.0; - return tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0); + return autoware_universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); } std::array convert_hex_string_to_decimal(const std::string & hex_str_color) @@ -198,7 +202,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( sub_save_map_ = create_subscription( "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { const auto lanelet2_output_file_path = - tier4_autoware_utils::getOrDeclareParameter( + autoware_universe_utils::getOrDeclareParameter( *this, "lanelet2_output_file_path"); if (!centerline_with_route_ || msg.data) { const auto & c = *centerline_with_route_; @@ -572,10 +576,10 @@ void StaticCenterlineGeneratorNode::evaluate( const std::vector & optimized_traj_points) { const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - const auto dist_thresh_vec = tier4_autoware_utils::getOrDeclareParameter>( + const auto dist_thresh_vec = autoware_universe_utils::getOrDeclareParameter>( *this, "marker_color_dist_thresh"); const auto marker_color_vec = - tier4_autoware_utils::getOrDeclareParameter>(*this, "marker_color"); + autoware_universe_utils::getOrDeclareParameter>(*this, "marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); @@ -622,13 +626,13 @@ void StaticCenterlineGeneratorNode::evaluate( // add footprint marker const auto footprint_marker = utils::create_footprint_marker(footprint_poly, marker_color, now(), i); - tier4_autoware_utils::appendMarkerArray(footprint_marker, &marker_array); + autoware_universe_utils::appendMarkerArray(footprint_marker, &marker_array); // add text of distance to bounds marker const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); const auto text_marker = utils::create_distance_text_marker(text_pose, min_dist_to_bound, marker_color, now(), i); - tier4_autoware_utils::appendMarkerArray(text_marker, &marker_array); + autoware_universe_utils::appendMarkerArray(text_marker, &marker_array); } } diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index 4a1b39387b4fc..88b4d3ffa14b4 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -15,7 +15,7 @@ #define TYPE_ALIAS_HPP_ #include "autoware/route_handler/route_handler.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" @@ -36,9 +36,9 @@ using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::Point2d; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index 7539cf69fae80..10b2589f3bf7b 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -16,8 +16,8 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include #include @@ -94,8 +94,8 @@ geometry_msgs::msg::Pose get_center_pose( // calculate middle pose geometry_msgs::msg::Pose middle_pose; middle_pose.position = middle_pos; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware_universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); return middle_pose; } @@ -189,11 +189,11 @@ MarkerArray create_footprint_marker( const double g = marker_color.at(1); const double b = marker_color.at(2); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints", idx, visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); + autoware_universe_utils::createMarkerScale(0.1, 0.0, 0.0), + autoware_universe_utils::createMarkerColor(r, g, b, 0.999)); marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); @@ -221,11 +221,11 @@ MarkerArray create_distance_text_marker( const double g = marker_color.at(1); const double b = marker_color.at(2); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); + autoware_universe_utils::createMarkerScale(0.5, 0.5, 0.5), + autoware_universe_utils::createMarkerColor(r, g, b, 0.999)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 530f850a3c34a..1984615333775 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_universe_utils autoware_vehicle_info_utils diagnostic_msgs eigen @@ -31,7 +32,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index 718e72845ade3..0559d39c7073c 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -14,9 +14,9 @@ #include "debug_marker.hpp" +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -52,12 +52,12 @@ Polygon2d createSelfPolygon( } } // namespace -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double base_link2front, diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 5375b1f736fd3..8a02c6d686736 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -14,9 +14,9 @@ #include "node.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -52,8 +52,8 @@ namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; using autoware_perception_msgs::msg::ObjectClassification; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::pose2transform; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::pose2transform; namespace { @@ -193,7 +193,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); @@ -251,12 +251,12 @@ rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( use_dynamic_object_ = false; for (const auto & label_pair : label_map_) { bool & check_current_label = node_param_.enable_check_map.at(label_pair.second); - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, label_pair.first + ".enable_check", check_current_label); use_dynamic_object_ = use_dynamic_object_ || check_current_label; } - tier4_autoware_utils::updateParam( + autoware_universe_utils::updateParam( parameters, "debug_footprint_label", node_param_.debug_footprint_label); const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); debug_ptr_->updateFootprintMargin( @@ -411,7 +411,7 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl tf2::transformToEigen(transform_stamped.value().transform).cast(); pcl::PointCloud transformed_pointcloud; pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( transformed_pointcloud, transformed_pointcloud, isometry); const double front_margin = node_param_.pointcloud_surround_check_front_distance; diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index b28d0fa6fff70..798470fe4568a 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -15,9 +15,9 @@ #ifndef NODE_HPP_ #define NODE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "debug_marker.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -111,11 +111,11 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // publisher and subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber - sub_pointcloud_{this, "~/input/pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ + autoware_universe_utils::InterProcessPollingSubscriber + sub_pointcloud_{this, "~/input/pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; + autoware_universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; @@ -143,7 +143,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; bool use_dynamic_object_; diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 106279c186db5..6f6ed564d5eab 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -15,6 +15,12 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/self_pose_listener.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" @@ -28,14 +34,8 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" -#include +#include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -88,13 +88,13 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; - tier4_autoware_utils::InterProcessPollingSubscriber sub_current_odometry_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_current_acceleration_{this, "~/input/acceleration"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{ - this, "~/input/external_velocity_limit_mps"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware_universe_utils::InterProcessPollingSubscriber + sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; + autoware_universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/operation_mode_state"}; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry @@ -245,7 +245,7 @@ class VelocitySmootherNode : public rclcpp::Node void initCommonParam(); // debug - tier4_autoware_utils::StopWatch stop_watch_; + autoware_universe_utils::StopWatch stop_watch_; std::shared_ptr prev_time_; double prev_acc_; rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; @@ -273,8 +273,8 @@ class VelocitySmootherNode : public rclcpp::Node void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); - std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 7dadd537b70aa..e8b866af673d0 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 11604da0dfa6c..1dd79054b975b 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 02fbba87b1a94..4651b95c54488 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 2bfc32ec90a5f..1a8d93fb29743 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_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation @@ -31,7 +32,6 @@ rclcpp tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 00df3ef6f3a54..c80ea2a67ba7e 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -14,11 +14,11 @@ #include "autoware/velocity_smoother/node.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" #include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "motion_utils/marker/marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -90,8 +90,9 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti clock_ = get_clock(); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void VelocitySmootherNode::setupSmoother(const double wheelbase) @@ -908,7 +909,7 @@ void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) } double distance_sum = 0.0; for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward - distance_sum += tier4_autoware_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); + distance_sum += autoware_universe_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); if (distance_sum > node_param_.stopping_distance) { break; } @@ -1019,7 +1020,7 @@ double VelocitySmootherNode::calcTravelDistance() const if (prev_closest_point_) { const double travel_dist = - tier4_autoware_utils::calcDistance2d(*prev_closest_point_, closest_point); + autoware_universe_utils::calcDistance2d(*prev_closest_point_, closest_point); return travel_dist; } diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp index 229f93980af70..00934f6648d7c 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -134,7 +134,7 @@ TrajectoryPoints resampleTrajectory( // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware_universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); @@ -253,7 +253,7 @@ TrajectoryPoints resampleTrajectory( // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware_universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 9d300487900a6..1124fedbdd285 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -252,7 +252,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( const auto tp1 = input.at(i + 1); const double dist_thr = 0.001; // 1mm - const double dist_tp0_tp1 = tier4_autoware_utils::calcDistance2d(tp0, tp1); + const double dist_tp0_tp1 = autoware_universe_utils::calcDistance2d(tp0, tp1); if (std::fabs(dist_tp0_tp1) < dist_thr) { output.push_back(input.at(i)); continue; @@ -355,7 +355,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt } if ( - tier4_autoware_utils::calcDistance2d(output.at(end_index), output.at(index)) < + autoware_universe_utils::calcDistance2d(output.at(end_index), output.at(index)) < dist_threshold) { end_index = index; min_latacc_velocity = std::min( @@ -441,7 +441,7 @@ bool AnalyticalJerkConstrainedSmoother::applyForwardJerkFilter( for (size_t i = start_index + 1; i < base_trajectory.size(); ++i) { const double prev_vel = output_trajectory.at(i - 1).longitudinal_velocity_mps; const double ds = - tier4_autoware_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); + autoware_universe_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); const double dt = ds / std::max(prev_vel, 1.0); const double prev_acc = output_trajectory.at(i - 1).acceleration_mps2; @@ -487,8 +487,8 @@ bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter( } } for (size_t i = decel_target_index; i > start_index; --i) { - dist += - tier4_autoware_utils::calcDistance2d(output_trajectory.at(i - 1), output_trajectory.at(i)); + dist += autoware_universe_utils::calcDistance2d( + output_trajectory.at(i - 1), output_trajectory.at(i)); dist_to_target.at(i - 1) = dist; } diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index c8802bd69d52c..fd9c4f34b4dc7 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -226,7 +226,7 @@ bool calcStopVelocityWithConstantJerkAccLimit( distances.push_back(distance); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { distance += - tier4_autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); + autoware_universe_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); if (distance > xs.back()) { break; } diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index a826b47136bd3..3761d16d7936e 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -385,7 +385,7 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( output.front().longitudinal_velocity_mps = current_vel; output.front().acceleration_mps2 = current_acc; for (size_t i = 1; i < input.size(); ++i) { - const double ds = tier4_autoware_utils::calcDistance2d(input.at(i), input.at(i - 1)); + const double ds = autoware_universe_utils::calcDistance2d(input.at(i), input.at(i - 1)); const double max_dt = std::pow(6.0 * ds / j_max, 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); @@ -440,7 +440,7 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( merged.at(i).acceleration_mps2 = current_acc; const double ds = - tier4_autoware_utils::calcDistance2d(forward_filtered.at(i + 1), forward_filtered.at(i)); + autoware_universe_utils::calcDistance2d(forward_filtered.at(i + 1), forward_filtered.at(i)); const double max_dt = std::pow(6.0 * ds / std::fabs(j_min), 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index f274444af88e2..d0e6ffa94df29 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -14,13 +14,13 @@ #include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include @@ -249,14 +249,15 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( } const auto steer_rate = steer_rate_arr.at(i); - if (steer_rate < tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate)) { + if (steer_rate < autoware_universe_utils::deg2rad(base_param_.max_steering_angle_rate)) { continue; } const auto mean_vel = (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; const auto target_mean_vel = - mean_vel * (tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); + mean_vel * + (autoware_universe_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); for (size_t k = 0; k < 2; k++) { auto & velocity = output.at(i + k).longitudinal_velocity_mps; diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index 305e651117515..35ba7d3972fd7 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -34,7 +34,7 @@ inline void convertEulerAngleToMonotonic(std::vector & a) { for (unsigned int i = 1; i < a.size(); ++i) { const double da = a[i] - a[i - 1]; - a[i] = a[i - 1] + tier4_autoware_utils::normalizeRadian(da); + a[i] = a[i - 1] + autoware_universe_utils::normalizeRadian(da); } } @@ -87,7 +87,7 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( { const auto & seg_pt = trajectory.at(seg_idx); const auto & next_pt = trajectory.at(seg_idx + 1); - traj_p.pose = tier4_autoware_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); + traj_p.pose = autoware_universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); traj_p.longitudinal_velocity_mps = interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = @@ -110,7 +110,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum = 0.0; for (size_t i = index; i < trajectory.size() - 1; ++i) { - dist_sum += tier4_autoware_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); + dist_sum += autoware_universe_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); if (dist_sum > ahead_length) { ahead_index = i + 1; break; @@ -123,7 +123,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum{0.0}; for (size_t i = index; i != 0; --i) { - dist_sum += tier4_autoware_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); + dist_sum += autoware_universe_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); if (dist_sum > behind_length) { behind_index = i - 1; break; @@ -152,7 +152,7 @@ std::vector calcArclengthArray(const TrajectoryPoints & trajectory) for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - dist += tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose); + dist += autoware_universe_utils::calcDistance2d(tp.pose, tp_prev.pose); arclength.at(i) = dist; } return arclength; @@ -164,7 +164,7 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - const double dist = tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose); + const double dist = autoware_universe_utils::calcDistance2d(tp.pose, tp_prev.pose); intervals.push_back(dist); } return intervals; @@ -173,8 +173,8 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using tier4_autoware_utils::calcCurvature; - using tier4_autoware_utils::getPoint; + using autoware_universe_utils::calcCurvature; + using autoware_universe_utils::getPoint; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); @@ -604,7 +604,7 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes // TODO(Horibe): use arc length distance const double stop_dist = - tier4_autoware_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); + autoware_universe_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); return stop_dist; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index 8bb9e96c881c1..a4561b1c960d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -23,11 +23,11 @@ autoware_behavior_path_planner_common autoware_behavior_path_static_obstacle_avoidance_module autoware_rtc_interface + autoware_universe_utils lanelet2_extension motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index a31316b4861c5..267963dc32681 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -15,9 +15,9 @@ #include "manager.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "data_structs.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -33,7 +33,7 @@ using autoware::behavior_path_planner::ObjectParameter; void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { using autoware_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::getOrDeclareParameter; + using autoware_universe_utils::getOrDeclareParameter; // init manager interface initInterface(node, {"left", "right"}); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 25c6f46bd4c72..769154ec9e105 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -195,10 +195,10 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( std::optional AvoidanceByLaneChange::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::calcLateralDeviation; using boost::geometry::return_centroid; using motion_utils::findNearestIndex; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::calcLateralDeviation; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index a42d273c512ba..53ecf19b8215f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include #include -#include #include #include @@ -58,7 +58,7 @@ std::vector getAllKeys(const std::unordered_map & map) namespace autoware::behavior_path_planner { using autoware_perception_msgs::msg::PredictedPath; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; struct MinMaxValue @@ -176,7 +176,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, const bool arg_is_object_on_ego_path, const std::optional & arg_latest_time_inside_ego_path) - : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), + : uuid(autoware_universe_utils::toHexString(predicted_object.object_id)), label(predicted_object.classification.front().label), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), shape(predicted_object.shape), @@ -374,8 +374,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface }; struct EgoPathReservePoly { - const tier4_autoware_utils::Polygon2d left_avoid; - const tier4_autoware_utils::Polygon2d right_avoid; + const autoware_universe_utils::Polygon2d left_avoid; + const autoware_universe_utils::Polygon2d right_avoid; }; bool canTransitSuccessState() override; @@ -431,11 +431,11 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const DynamicAvoidanceObject & object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; - std::optional calcEgoPathBasedDynamicObstaclePolygon( + std::optional calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcObjectPathBasedDynamicObstaclePolygon( + std::optional calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcPredictedPathBasedDynamicObstaclePolygon( + std::optional calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const; EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const; @@ -455,7 +455,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface TargetObjectsManager target_objects_manager_; - mutable tier4_autoware_utils::StopWatch< + mutable autoware_universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index a22e34f20a922..9ec4b0c716506 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_path_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_msgs geometry_msgs lanelet2_core @@ -29,7 +30,6 @@ pluginlib rclcpp signal_processing - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index c67a90bdc2d8b..9b0db95bb0df5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -152,7 +152,7 @@ void DynamicObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void DynamicObstacleAvoidanceModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto & p = parameters_; { // common diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index d230f58b45e06..2643166350a19 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -41,7 +41,7 @@ namespace autoware::behavior_path_planner { namespace { -geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point) +geometry_msgs::msg::Point toGeometryPoint(const autoware_universe_utils::Point2d & point) { geometry_msgs::msg::Point geom_obj_point; geom_obj_point.x = point.x(); @@ -65,24 +65,25 @@ MinMaxValue combineMinMaxValues(const MinMaxValue & r1, const MinMaxValue & r2) void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) { - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, - tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + autoware_universe_utils::createMarkerScale(3.0, 1.0, 1.0), + autoware_universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); marker.pose = obj_pose; marker_array.markers.push_back(marker); } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly, const double obj_z) + MarkerArray & marker_array, const autoware_universe_utils::Polygon2d & obj_poly, + const double obj_z) { - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + autoware_universe_utils::createMarkerScale(0.1, 0.0, 0.0), + autoware_universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. for (size_t i = 0; i < obj_poly.outer().size(); ++i) { @@ -176,7 +177,7 @@ double calcDiffAngleAgainstPath( const double target_yaw = tf2::getYaw(target_pose.orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware_universe_utils::normalizeRadian(target_yaw - traj_yaw); return diff_yaw; } @@ -191,7 +192,7 @@ double calcDiffAngleAgainstPath( double signed_max_angle{0.0}; for (size_t i = 0; i < std::min(max_predicted_path_size, predicted_path.path.size()); ++i) { const double obj_yaw = tf2::getYaw(predicted_path.path.at(i).orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw); + const double diff_yaw = autoware_universe_utils::normalizeRadian(obj_yaw - ego_yaw); if (std::abs(signed_max_angle) < std::abs(diff_yaw)) { signed_max_angle = diff_yaw; } @@ -206,14 +207,15 @@ double calcDistanceToPath( const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); if (target_idx == 0 || target_idx == path_points.size() - 1) { const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); - const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + const double angle_to_target_pos = autoware_universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + const double diff_yaw = + autoware_universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); if ( (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || (target_idx == path_points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { - return tier4_autoware_utils::calcDistance2d(path_points.at(target_idx), target_pos); + return autoware_universe_utils::calcDistance2d(path_points.at(target_idx), target_pos); } } @@ -226,9 +228,10 @@ bool isLeft( { const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); - const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + const double angle_to_target_pos = autoware_universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + const double diff_yaw = + autoware_universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); if (0 < diff_yaw) { return true; @@ -279,7 +282,7 @@ std::optional> intersectLines( ++source_seg_idx) { for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; ++target_seg_idx) { - const auto intersect_point = tier4_autoware_utils::intersect( + const auto intersect_point = autoware_universe_utils::intersect( source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); if (intersect_point) { @@ -494,7 +497,7 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware_universe_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -582,7 +585,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware_universe_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -784,7 +787,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by // "ego_path_base" - const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware_universe_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); @@ -887,19 +890,19 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( const double y = feasible_lat_offset; const auto feasible_left_bound_point = - tier4_autoware_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + autoware_universe_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); const auto feasible_right_bound_point = - tier4_autoware_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + autoware_universe_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); } - tier4_autoware_utils::appendMarkerArray( + autoware_universe_utils::appendMarkerArray( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), &debug_marker_); - tier4_autoware_utils::appendMarkerArray( + autoware_universe_utils::appendMarkerArray( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), &debug_marker_); @@ -943,7 +946,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( std::optional collision_start_idx{std::nullopt}; double lon_dist = 0.0; for (size_t i = ego_idx; i < ego_path.size() - 1; ++i) { - lon_dist += tier4_autoware_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); + lon_dist += autoware_universe_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); const double elapsed_time = lon_dist / ego_vel; const auto future_ego_pose = ego_path.at(i); @@ -952,7 +955,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( if (future_obj_pose) { const double dist_ego_to_obj = - tier4_autoware_utils::calcDistance2d(future_ego_pose, *future_obj_pose); + autoware_universe_utils::calcDistance2d(future_ego_pose, *future_obj_pose); if (dist_ego_to_obj < 1.0) { if (!collision_start_idx) { collision_start_idx = i; @@ -1186,7 +1189,7 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const autoware_perception_msgs::msg::Shape & obj_shape) const { const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); - const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_points = autoware_universe_utils::toPolygon2d(obj_pose, obj_shape); // TODO(murooka) calculation is not so accurate. std::vector obj_lat_offset_vec; @@ -1366,7 +1369,7 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx); const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx); if (prev_min_dist && next_min_dist) { - const double segment_length = tier4_autoware_utils::calcDistance2d( + const double segment_length = autoware_universe_utils::calcDistance2d( obj_path.path.at(prev_valid_obj_path_end_idx), obj_path.path.at(next_valid_obj_path_end_idx)); const double partial_segment_length = segment_length * @@ -1503,7 +1506,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( }(); const auto obj_occupancy_region = [&]() { - const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware_universe_utils::toPolygon2d(object.pose, object.shape); std::vector lat_pos_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); @@ -1559,7 +1562,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( } // NOTE: object does not have const only to update min_bound_lat_offset. -std::optional +std::optional DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1571,7 +1574,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, object.pose.position); - // const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + // const auto obj_points = autoware_universe_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly); @@ -1595,7 +1598,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. - obj_inner_bound_poses.push_back(tier4_autoware_utils::calcOffsetPose( + obj_inner_bound_poses.push_back(autoware_universe_utils::calcOffsetPose( ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, 0.0)); } @@ -1611,7 +1614,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( // Check if the object polygon intersects with the ego_lat_feasible_path. if (intersect_result) { const auto & [bound_seg_idx, intersect_point] = *intersect_result; - const double lon_offset = tier4_autoware_utils::calcDistance2d( + const double lon_offset = autoware_universe_utils::calcDistance2d( obj_inner_bound_poses.at(bound_seg_idx), intersect_point); const auto obj_inner_bound_start_idx_opt = @@ -1643,17 +1646,17 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector feasible_obj_outer_bound_points; for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { feasible_obj_outer_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( + autoware_universe_utils::calcOffsetPose( feasible_obj_inner_bound_pose, 0.0, object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) .position); } // create obj_polygon from inner/outer bound points - tier4_autoware_utils::Polygon2d obj_poly; + autoware_universe_utils::Polygon2d obj_poly; const auto add_points_to_obj_poly = [&](const auto & bound_points) { for (const auto & bound_point : bound_points) { - obj_poly.outer().push_back(tier4_autoware_utils::Point2d(bound_point.x, bound_point.y)); + obj_poly.outer().push_back(autoware_universe_utils::Point2d(bound_point.x, bound_point.y)); } }; add_points_to_obj_poly(feasible_obj_inner_bound_points); @@ -1665,7 +1668,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( } // should be replace by the function calcPredictedPathBasedDynamicObstaclePolygon() (takagi) -std::optional +std::optional DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1694,26 +1697,26 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const auto & obj_pose = obj_path.path.at(i); obj_left_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( + autoware_universe_utils::calcOffsetPose( obj_pose, lon_offset, object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0) .position); obj_right_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( + autoware_universe_utils::calcOffsetPose( obj_pose, lon_offset, -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 0.0) .position); } // create obj_polygon from inner/outer bound points - tier4_autoware_utils::Polygon2d obj_poly; + autoware_universe_utils::Polygon2d obj_poly; for (const auto & bound_point : obj_right_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware_universe_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end()); for (const auto & bound_point : obj_left_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware_universe_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } @@ -1724,7 +1727,7 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // Calculate polygons according to predicted_path with certain confidence, // except for the area required for ego safety. // input: an object, the minimum area required for ego safety, and some global params -std::optional +std::optional DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const { @@ -1740,10 +1743,10 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( } } - tier4_autoware_utils::Polygon2d obj_points_as_poly; + autoware_universe_utils::Polygon2d obj_points_as_poly; for (const auto pose : obj_poses) { boost::geometry::append( - obj_points_as_poly, tier4_autoware_utils::toFootprint( + obj_points_as_poly, autoware_universe_utils::toFootprint( pose, object.shape.dimensions.x * 0.5, object.shape.dimensions.x * 0.5, object.shape.dimensions.y * 0.5) .outer()); @@ -1752,7 +1755,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( Polygon2d obj_poly; boost::geometry::convex_hull(obj_points_as_poly, obj_poly); - tier4_autoware_utils::MultiPolygon2d expanded_poly; + autoware_universe_utils::MultiPolygon2d expanded_poly; namespace strategy = boost::geometry::strategy::buffer; boost::geometry::buffer( obj_poly, expanded_poly, @@ -1761,7 +1764,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( strategy::point_circle()); if (expanded_poly.empty()) return {}; - tier4_autoware_utils::MultiPolygon2d output_poly; + autoware_universe_utils::MultiPolygon2d output_poly; boost::geometry::difference( expanded_poly[0], object.is_collision_left ? ego_path_poly.right_avoid : ego_path_poly.left_avoid, output_poly); @@ -1797,26 +1800,27 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg assert(!ego_path.points.empty()); - tier4_autoware_utils::LineString2d ego_path_lines; + autoware_universe_utils::LineString2d ego_path_lines; for (const auto & path_point : ego_path.points) { - ego_path_lines.push_back(tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d()); + ego_path_lines.push_back( + autoware_universe_utils::fromMsg(path_point.point.pose.position).to_2d()); } auto calcReservePoly = [&ego_path_lines]( const strategy::distance_asymmetric path_expand_strategy, const strategy::distance_asymmetric steer_expand_strategy, const std::vector & outer_body_path) - -> tier4_autoware_utils::Polygon2d { + -> autoware_universe_utils::Polygon2d { // reserve area based on the reference path - tier4_autoware_utils::MultiPolygon2d path_poly; + autoware_universe_utils::MultiPolygon2d path_poly; boost::geometry::buffer( ego_path_lines, path_poly, path_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); // reserve area steer to the avoidance path - tier4_autoware_utils::LineString2d steer_lines; + autoware_universe_utils::LineString2d steer_lines; for (const auto & point : outer_body_path) { - const auto bg_point = tier4_autoware_utils::fromMsg(point).to_2d(); + const auto bg_point = autoware_universe_utils::fromMsg(point).to_2d(); if (boost::geometry::within(bg_point, path_poly)) { if (steer_lines.size() != 0) { ; @@ -1826,12 +1830,12 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg } // boost::geometry::append(steer_lines, bg_point); } - tier4_autoware_utils::MultiPolygon2d steer_poly; + autoware_universe_utils::MultiPolygon2d steer_poly; boost::geometry::buffer( steer_lines, steer_poly, steer_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); - tier4_autoware_utils::MultiPolygon2d output_poly; + autoware_universe_utils::MultiPolygon2d output_poly; boost::geometry::union_(path_poly, steer_poly, output_poly); if (output_poly.size() != 1) { assert(false); @@ -1845,11 +1849,11 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg const double vehicle_half_width = planner_data_->parameters.vehicle_width * 0.5; const double reserve_width_obj_side = vehicle_half_width - parameters_->max_lat_offset_to_avoid; - const tier4_autoware_utils::Polygon2d left_avoid_poly = calcReservePoly( + const autoware_universe_utils::Polygon2d left_avoid_poly = calcReservePoly( strategy::distance_asymmetric(vehicle_half_width, reserve_width_obj_side), strategy::distance_asymmetric(vehicle_half_width, 0.0), motion_saturated_outer_paths.right_path); - const tier4_autoware_utils::Polygon2d right_avoid_poly = calcReservePoly( + const autoware_universe_utils::Polygon2d right_avoid_poly = calcReservePoly( strategy::distance_asymmetric(reserve_width_obj_side, vehicle_half_width), strategy::distance_asymmetric(0.0, vehicle_half_width), motion_saturated_outer_paths.left_path); diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index b794789bd19e0..83ddcfe989061 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -22,10 +22,10 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_rtc_interface + autoware_universe_utils motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 528a4b098720c..dc8a60c9a8373 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -22,8 +22,8 @@ #include +using autoware_universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 904875b5cbdc3..0097a6d6e0b4a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -31,8 +31,8 @@ #include #include #include +#include #include -#include #include #include @@ -69,7 +69,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Polygon2d; #define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ public: \ @@ -387,7 +387,7 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr ego_predicted_path_params; std::shared_ptr objects_filtering_params; std::shared_ptr safety_check_params; - tier4_autoware_utils::LinearRing2d vehicle_footprint; + autoware_universe_utils::LinearRing2d vehicle_footprint; PlannerData planner_data; ModuleStatus current_status; @@ -409,7 +409,7 @@ class GoalPlannerModule : public SceneModuleInterface const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, const std::shared_ptr goal_searcher_, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint); + const autoware_universe_utils::LinearRing2d & vehicle_footprint); private: void initializeOccupancyGridMap( @@ -484,7 +484,7 @@ class GoalPlannerModule : public SceneModuleInterface std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; - tier4_autoware_utils::LinearRing2d vehicle_footprint_; + autoware_universe_utils::LinearRing2d vehicle_footprint_; std::recursive_mutex mutex_; // TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 3c6b0095f67f4..38c55b0b87ecb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -23,7 +23,7 @@ namespace autoware::behavior_path_planner { -using tier4_autoware_utils::LinearRing2d; +using autoware_universe_utils::LinearRing2d; using BasicPolygons2d = std::vector; class GoalSearcher : public GoalSearcherBase diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp index 70de7e87496f5..4a1a925eb114c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -26,8 +26,8 @@ namespace autoware::behavior_path_planner { +using autoware_universe_utils::MultiPolygon2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::MultiPolygon2d; struct GoalCandidate { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ebee1d7f330d6..5cbe3bc4ee84c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -25,8 +25,8 @@ #include #include +using autoware_universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 80c43de08006e..927145fc69db1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include @@ -42,7 +42,7 @@ using geometry_msgs::msg::Twist; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Polygon2d = autoware_universe_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -94,7 +94,7 @@ std::vector createPathFootPrints( // debug MarkerArray createPullOverAreaMarkerArray( - const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const autoware_universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z); MarkerArray createPosesMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index 952173564888a..148a3f966303b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -23,10 +23,10 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_rtc_interface + autoware_universe_utils motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index f95d89f1cd8f2..b5dac4264de39 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -25,7 +25,7 @@ namespace autoware::behavior_path_planner { -using Point2d = tier4_autoware_utils::Point2d; +using Point2d = autoware_universe_utils::Point2d; using tier4_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp index 9831de75289e3..64d141e202b9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -57,7 +57,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) constexpr double straight_distance = 1.0; const Pose end_pose = use_back_ ? goal_pose - : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); + : autoware_universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); if (!planner_->makePlan(current_pose, end_pose)) { return {}; } @@ -85,7 +85,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware_universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); if (distance < th_goal_distance) { last_path.points.erase(it, last_path.points.end()); break; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 34f431c692e44..79078e15d4716 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -22,8 +22,8 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -40,15 +40,15 @@ #include using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::inverseTransformPose; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using motion_utils::insertDecelPoint; using nav_msgs::msg::OccupancyGrid; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::inverseTransformPose; namespace autoware::behavior_path_planner { @@ -1764,7 +1764,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath() const auto current_path_end = thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; - return tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < + return autoware_universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; } @@ -1877,7 +1877,7 @@ bool GoalPlannerModule::checkObjectsCollision( std::vector obj_polygons; for (const auto & object : target_objects.objects) { - obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); + obj_polygons.push_back(autoware_universe_utils::toPolygon2d(object)); } /* Expand ego collision check polygon @@ -1901,7 +1901,7 @@ bool GoalPlannerModule::checkObjectsCollision( extra_stopping_margin, std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto ego_polygon = tier4_autoware_utils::toFootprint( + const auto ego_polygon = autoware_universe_utils::toFootprint( p.point.pose, planner_data->parameters.base_link2front + collision_check_margin + extra_stopping_margin, planner_data->parameters.base_link2rear + collision_check_margin, @@ -2199,7 +2199,7 @@ static std::vector filterOb for (const auto & target_lane : target_lanes) { const auto lane_poly = target_lane.polygon2d().basicPolygon(); for (const auto & filtered_object : filtered_objects.objects) { - const auto object_bbox = tier4_autoware_utils::toPolygon2d(filtered_object); + const auto object_bbox = autoware_universe_utils::toPolygon2d(filtered_object); if (boost::geometry::within(object_bbox, lane_poly)) { within_filtered_objects.push_back(filtered_object); } @@ -2282,7 +2282,7 @@ std::pair GoalPlannerModule::isSafePath( lanelet::utils::conversion::toGeomMsgPt(fist_road_lane.centerline().front()); const double lane_yaw = lanelet::utils::getLaneletAngle(fist_road_lane, first_road_point); first_road_pose.position = first_road_point; - first_road_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + first_road_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(lane_yaw); // if current ego pose is before pull over lanes segment, use first road lanelet center pose if ( calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) < @@ -2359,6 +2359,9 @@ void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using autoware_universe_utils::createDefaultMarker; + using autoware_universe_utils::createMarkerColor; + using autoware_universe_utils::createMarkerScale; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; @@ -2367,9 +2370,6 @@ void GoalPlannerModule::setDebugData() using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; using motion_utils::createStopVirtualWallMarker; - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; const auto header = planner_data_->route_handler->getRouteHeader(); @@ -2377,7 +2377,7 @@ void GoalPlannerModule::setDebugData() for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + autoware_universe_utils::appendMarkerArray(added, &debug_marker_); }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas @@ -2425,10 +2425,10 @@ void GoalPlannerModule::setDebugData() createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware_universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, - tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + autoware_universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware_universe_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); const double ego_z = planner_data_->self_odometry->pose.pose.position.z; for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { @@ -2436,19 +2436,19 @@ void GoalPlannerModule::setDebugData() const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); marker.points.push_back( - tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z)); + autoware_universe_utils::createPoint(current_point.x(), current_point.y(), ego_z)); marker.points.push_back( - tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + autoware_universe_utils::createPoint(next_point.x(), next_point.y(), ego_z)); } } debug_marker_.markers.push_back(marker); if (parameters_->safety_check_params.enable_safety_check) { - tier4_autoware_utils::appendMarkerArray( + autoware_universe_utils::appendMarkerArray( goal_planner_utils::createLaneletPolygonMarkerArray( debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, "expanded_pull_over_lane_between_ego", - tier4_autoware_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), + autoware_universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), &debug_marker_); } @@ -2575,7 +2575,7 @@ void GoalPlannerModule::printParkingPositionError() const real_shoulder_to_map_shoulder + parameters_->margin_from_boundary - dy; RCLCPP_INFO( getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, - tier4_autoware_utils::rad2deg( + autoware_universe_utils::rad2deg( tf2::getYaw(current_pose.orientation) - tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); @@ -2631,7 +2631,7 @@ void GoalPlannerModule::GoalPlannerData::update( const PlannerData & planner_data_, const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, const std::shared_ptr goal_searcher_, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint_) + const autoware_universe_utils::LinearRing2d & vehicle_footprint_) { parameters = parameters_; ego_predicted_path_params = ego_predicted_path_params_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index a0b6cc587f929..87d1034f20682 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -18,10 +18,10 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -33,9 +33,9 @@ namespace autoware::behavior_path_planner { using autoware::lane_departure_checker::LaneDepartureChecker; +using autoware_universe_utils::calcOffsetPose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; -using tier4_autoware_utils::calcOffsetPose; // Sort with smaller longitudinal distances taking precedence over smaller lateral distances. struct SortByLongitudinalDistance @@ -136,7 +136,7 @@ GoalCandidates GoalSearcher::search( // todo(kosuke55): fix orientation for inverseTransformPoint temporarily Pose center_pose = p.point.pose; center_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); + autoware_universe_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -167,7 +167,7 @@ GoalCandidates GoalSearcher::search( search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); + transformVector(vehicle_footprint_, autoware_universe_utils::pose2transform(search_pose)); if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { // break here to exclude goals located laterally in no_parking_areas @@ -245,8 +245,8 @@ void GoalSearcher::countObjectsToAvoid( for (const auto & object : objects.objects) { for (const auto & p : current_center_line_path.points) { const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(p.point.pose)); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + transformVector(vehicle_footprint_, autoware_universe_utils::pose2transform(p.point.pose)); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, transformed_vehicle_footprint); if (distance > parameters_.object_recognition_collision_check_hard_margins.back()) { continue; @@ -431,9 +431,9 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( void GoalSearcher::createAreaPolygons( std::vector original_search_poses, const std::shared_ptr & planner_data) { - using tier4_autoware_utils::MultiPolygon2d; - using tier4_autoware_utils::Point2d; - using tier4_autoware_utils::Polygon2d; + using autoware_universe_utils::MultiPolygon2d; + using autoware_universe_utils::Point2d; + using autoware_universe_utils::Polygon2d; const double vehicle_width = planner_data->parameters.vehicle_width; const double base_link2front = planner_data->parameters.base_link2front; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 0e8e8c51dfc0a..098405f2378b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -15,7 +15,7 @@ #include "autoware/behavior_path_goal_planner_module/manager.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -428,7 +428,7 @@ void GoalPlannerModuleManager::updateModuleParams( // object_recognition_collision_check_hard_margins, maximum_deceleration, shift_sampling_num or // parking_policy, there seems to be a problem when we use a temp value to check these values. - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto & p = parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index e68cf4b9ebaeb..8cd368106f8dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -114,7 +114,7 @@ std::optional ShiftPullOver::cropPrevModulePath( const double offset = motion_utils::calcSignedArcLength( prev_module_path.points, shift_end_idx, shift_end_pose.position); projected_point.point.pose = - tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + autoware_universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); auto clipped_prev_module_path = prev_module_path; clipped_prev_module_path.points = clipped_points; @@ -131,7 +131,7 @@ std::optional ShiftPullOver::generatePullOverPath( // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = - tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); + autoware_universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( @@ -162,7 +162,7 @@ std::optional ShiftPullOver::generatePullOverPath( const Pose & shift_end_pose_prev_module_path = processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - tier4_autoware_utils::inverseTransformPoint( + autoware_universe_utils::inverseTransformPoint( shift_end_pose.position, shift_end_pose_prev_module_path) .y; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index a385b9cff20b2..81aa6c352fe4c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -17,11 +17,11 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include #include -#include #include @@ -38,11 +38,11 @@ namespace autoware::behavior_path_planner::goal_planner_utils { -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -116,9 +116,9 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const double ego_length_to_front = wheel_base + front_overhang; const double ego_width_to_front = !left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang); - tier4_autoware_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; - const auto front_edge_glob = tier4_autoware_utils::transformPoint( - front_edge_local, tier4_autoware_utils::pose2transform(ego_pose)); + autoware_universe_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; + const auto front_edge_glob = autoware_universe_utils::transformPoint( + front_edge_local, autoware_universe_utils::pose2transform(ego_pose)); geometry_msgs::msg::Pose ego_front_pose; ego_front_pose.position = createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z); @@ -180,7 +180,7 @@ PredictedObjects filterObjectsByLateralDistance( } MarkerArray createPullOverAreaMarkerArray( - const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const autoware_universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) { MarkerArray marker_array{}; @@ -205,7 +205,7 @@ MarkerArray createPosesMarkerArray( MarkerArray msg{}; int32_t i = 0; for (const auto & pose : poses) { - Marker marker = tier4_autoware_utils::createDefaultMarker( + Marker marker = autoware_universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, createMarkerScale(0.5, 0.25, 0.25), color); marker.pose = pose; @@ -309,7 +309,7 @@ double calcLateralDeviationBetweenPaths( motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); lateral_deviation = std::max( lateral_deviation, - std::abs(tier4_autoware_utils::calcLateralDeviation( + std::abs(autoware_universe_utils::calcLateralDeviation( reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); } return lateral_deviation; @@ -336,7 +336,7 @@ std::optional cropPath(const PathWithLaneId & path, const Pose & PathPointWithLaneId projected_point = clipped_points.back(); const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); projected_point.point.pose = - tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + autoware_universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); auto clipped_path = path; clipped_path.points = clipped_points; @@ -351,7 +351,8 @@ PathWithLaneId cropForwardPoints( double sum_length = 0; for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + const double seg_length = + autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length + seg_length) { const auto cropped_points = std::vector{points.begin() + target_seg_idx, points.begin() + i}; @@ -400,17 +401,17 @@ PathWithLaneId extendPath( const double lateral_shift_from_reference_path = motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); for (auto & p : clipped_path.points) { - p.point.pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); + p.point.pose = autoware_universe_utils::calcOffsetPose( + p.point.pose, 0, lateral_shift_from_reference_path, 0); } auto extended_path = target_path; const auto start_point = std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { const bool is_forward = - tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x > - 0.0; - const bool is_close = tier4_autoware_utils::calcDistance2d( + autoware_universe_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose) + .x > 0.0; + const bool is_close = autoware_universe_utils::calcDistance2d( p.point.pose.position, target_terminal_pose.position) < 0.1; return is_forward && !is_close; }); @@ -442,7 +443,7 @@ std::vector createPathFootPrints( for (const auto & point : path.points) { const auto & pose = point.point.pose; footprints.push_back( - tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + autoware_universe_utils::toFootprint(pose, base_to_front, base_to_rear, width)); } return footprints; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 0da1bcfda3370..89f5f715549bd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -21,7 +21,7 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -37,11 +37,11 @@ namespace autoware::behavior_path_planner { using autoware::route_handler::Direction; +using autoware_universe_utils::StopWatch; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeBase diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 4a9573463d73d..3162b99b441d1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -23,7 +23,7 @@ #include "rclcpp/logger.hpp" #include -#include +#include #include #include @@ -47,13 +47,13 @@ using autoware::route_handler::Direction; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using autoware_universe_utils::Polygon2d; using data::lane_change::LanesPolygon; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index ca338771064b9..8ea741f74ba13 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -23,10 +23,10 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_rtc_interface + autoware_universe_utils motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 0f7413eed1ca5..1c0f12549b863 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -20,7 +20,7 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" -#include +#include #include #include @@ -29,7 +29,7 @@ namespace autoware::behavior_path_planner { -using tier4_autoware_utils::appendMarkerArray; +using autoware_universe_utils::appendMarkerArray; using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index b4cf3630d9d88..1e066d297f878 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "autoware/behavior_path_lane_change_module/manager.hpp" #include "autoware/behavior_path_lane_change_module/interface.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -36,7 +36,7 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) void LaneChangeModuleManager::initParams(rclcpp::Node * node) { - using tier4_autoware_utils::getOrDeclareParameter; + using autoware_universe_utils::getOrDeclareParameter; LaneChangeParameters p{}; @@ -281,7 +281,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto p = parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 1c391c10aee92..83198b5328c86 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -22,9 +22,9 @@ #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include -#include #include #include @@ -389,9 +389,9 @@ void NormalLaneChange::insertStopPoint( // calculate distance from path front to the stationary object polygon on the ego lane. const auto polygon = - tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + autoware_universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); for (const auto & polygon_p : polygon) { - const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto p_fp = autoware_universe_utils::toMsg(polygon_p.to_3d()); const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); // ignore if the point is around the ego path @@ -442,7 +442,7 @@ void NormalLaneChange::insertStopPoint( // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( - tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + autoware_universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), lanelet::utils::combineLaneletsShape(status_.target_lanes) .polygon2d() .basicPolygon())) { @@ -991,11 +991,11 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( } const auto is_ahead_of_ego = [&path, ¤t_pose](const auto & object) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer(); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(object).outer(); double max_dist_ego_to_obj = std::numeric_limits::lowest(); for (const auto & polygon_p : obj_polygon) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware_universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); } @@ -1083,11 +1083,11 @@ void NormalLaneChange::filterAheadTerminalObjects( // ignore object that are ahead of terminal lane change start utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer(); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(object).outer(); // ignore object that are ahead of terminal lane change start auto distance_to_terminal_from_object = std::numeric_limits::max(); for (const auto & polygon_p : obj_polygon) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware_universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); Pose polygon_pose; polygon_pose.position = obj_p; polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; @@ -2079,7 +2079,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index e6720135a666a..78db0ba0a1b4f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include +#include #include -#include #include #include @@ -34,9 +34,9 @@ namespace marker_utils::lane_change_markers { +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerScale; using geometry_msgs::msg::Point; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerScale; MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { @@ -172,7 +172,7 @@ MarkerArray createDebugMarkerArray( MarkerArray debug_marker; const auto add = [&debug_marker](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker); + autoware_universe_utils::appendMarkerArray(added, &debug_marker); }; if (!debug_data.execution_area.points.empty()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 77eeccba74bca..30d943deb791a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -22,9 +22,11 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include +#include #include #include #include @@ -32,8 +34,6 @@ #include #include #include -#include -#include #include @@ -60,10 +60,10 @@ namespace autoware::behavior_path_planner::utils::lane_change using autoware::route_handler::RouteHandler; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; using lanelet::ArcCoordinates; @@ -404,13 +404,13 @@ std::optional constructCandidatePath( std::prev(prepare_segment.points.end() - 1)->point.pose; const auto & lane_change_start_from_shifted = std::next(shifted_path.path.points.begin())->point.pose; - const auto yaw_diff2 = std::abs(tier4_autoware_utils::normalizeRadian( + const auto yaw_diff2 = std::abs(autoware_universe_utils::normalizeRadian( tf2::getYaw(prepare_segment_second_last_point.orientation) - tf2::getYaw(lane_change_start_from_shifted.orientation))); - if (yaw_diff2 > tier4_autoware_utils::deg2rad(5.0)) { + if (yaw_diff2 > autoware_universe_utils::deg2rad(5.0)) { RCLCPP_DEBUG( get_logger(), "Excessive yaw difference %.3f which exceeds the 5 degrees threshold.", - tier4_autoware_utils::rad2deg(yaw_diff2)); + autoware_universe_utils::rad2deg(yaw_diff2)); return std::nullopt; } } @@ -935,7 +935,7 @@ bool isParkedObject( const auto & obj_pose = object.initial_pose.pose; const auto & obj_shape = object.shape; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_poly = autoware_universe_utils::toPolygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; double max_dist_to_bound = std::numeric_limits::lowest(); @@ -994,7 +994,7 @@ bool passParkedObject( const auto & leading_obj = objects.at(*leading_obj_idx); auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = - tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); + autoware_universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return false; } @@ -1002,7 +1002,7 @@ bool passParkedObject( const auto & current_path_end = current_lane_path.points.back().point.pose.position; double min_dist_to_end_of_current_lane = std::numeric_limits::max(); for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); + const auto obj_p = autoware_universe_utils::createPoint(point.x(), point.y(), 0.0); const double dist = motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); @@ -1122,7 +1122,7 @@ ExtendedPredictedObject transform( } const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( t, *obj_pose, obj_vel_norm, obj_polygon); } @@ -1165,7 +1165,7 @@ Polygon2d getEgoCurrentFootprint( const auto base_to_rear = ego_info.rear_overhang_m; const auto width = ego_info.vehicle_width_m; - return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); + return autoware_universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); } bool isWithinIntersection( @@ -1252,10 +1252,14 @@ geometry_msgs::msg::Polygon createExecutionArea( const double backward_lon_offset = -base_to_rear; const double lat_offset = width / 2.0 + additional_lat_offset; - const auto p1 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + const auto p1 = + autoware_universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = + autoware_universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = + autoware_universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = + autoware_universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); geometry_msgs::msg::Polygon polygon; polygon.points.push_back(create_point32(p1)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 096828fdbf120..a31172ac0050b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include -#include +#include +#include #include #include @@ -24,16 +24,16 @@ constexpr double epsilon = 1e-6; TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) { geometry_msgs::msg::Pose ego_pose; - const auto ego_yaw = tier4_autoware_utils::deg2rad(0.0); - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); - ego_pose.position = tier4_autoware_utils::createPoint(0, 0, 0); + const auto ego_yaw = autoware_universe_utils::deg2rad(0.0); + ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.position = autoware_universe_utils::createPoint(0, 0, 0); geometry_msgs::msg::Pose obj_pose; - const auto obj_yaw = tier4_autoware_utils::deg2rad(0.0); - obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(obj_yaw); - obj_pose.position = tier4_autoware_utils::createPoint(-4, 3, 0); + const auto obj_yaw = autoware_universe_utils::deg2rad(0.0); + obj_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(obj_yaw); + obj_pose.position = autoware_universe_utils::createPoint(-4, 3, 0); - const auto result = tier4_autoware_utils::inverseTransformPose(obj_pose, ego_pose); + const auto result = autoware_universe_utils::inverseTransformPose(obj_pose, ego_pose); EXPECT_NEAR(result.position.x, -4, epsilon); EXPECT_NEAR(result.position.y, 3, epsilon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index 65fe99ce5d775..1420926f32bd7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -18,11 +18,11 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/interface/steering_factor_interface.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include #include #include @@ -88,30 +88,30 @@ class BehaviorPathPlannerNode : public rclcpp::Node private: // subscriber - tier4_autoware_utils::InterProcessPollingSubscriber route_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber route_subscriber_{ this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - tier4_autoware_utils::InterProcessPollingSubscriber vector_map_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - tier4_autoware_utils::InterProcessPollingSubscriber velocity_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber velocity_subscriber_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber acceleration_subscriber_{this, "~/input/accel"}; - tier4_autoware_utils::InterProcessPollingSubscriber scenario_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber scenario_subscriber_{ this, "~/input/scenario"}; - tier4_autoware_utils::InterProcessPollingSubscriber perception_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber perception_subscriber_{ this, "~/input/perception"}; - tier4_autoware_utils::InterProcessPollingSubscriber occupancy_grid_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber occupancy_grid_subscriber_{ this, "~/input/occupancy_grid_map"}; - tier4_autoware_utils::InterProcessPollingSubscriber costmap_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber costmap_subscriber_{ this, "~/input/costmap"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber traffic_signals_subscriber_{this, "~/input/traffic_signals"}; - tier4_autoware_utils::InterProcessPollingSubscriber lateral_offset_subscriber_{ + autoware_universe_utils::InterProcessPollingSubscriber lateral_offset_subscriber_{ this, "~/input/lateral_offset"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber operation_mode_subscriber_{ this, "/system/operation_mode/state", rclcpp::QoS{1}.transient_local()}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber external_limit_max_velocity_subscriber_{this, "/planning/scenario_planning/max_velocity"}; // publisher @@ -236,9 +236,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 5381d201ffeaa..ecd9dd37c363a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -18,8 +18,8 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -39,12 +39,12 @@ namespace autoware::behavior_path_planner { -using tier4_autoware_utils::StopWatch; +using autoware_universe_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; -using DebugPublisher = tier4_autoware_utils::DebugPublisher; +using DebugPublisher = autoware_universe_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 2efc32531432b..639d0c9ca084e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -45,6 +45,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs behaviortree_cpp_v3 @@ -65,7 +66,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index e485ceb70f150..6878fcc083973 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_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 @@ -114,8 +114,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -471,7 +472,7 @@ void BehaviorPathPlannerNode::run() if ( output.modified_goal && /* has changed modified goal */ ( - !planner_data_->prev_modified_goal || tier4_autoware_utils::calcDistance2d( + !planner_data_->prev_modified_goal || autoware_universe_utils::calcDistance2d( planner_data_->prev_modified_goal->pose.position, output.modified_goal->pose.position) > 0.01)) { PoseWithUuidStamped modified_goal = *(output.modified_goal); @@ -565,27 +566,29 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb constexpr double scale_x = 1.0; constexpr double scale_y = 1.0; constexpr double scale_z = 1.0; - const auto scale = tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z); - const auto desired_section_color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); - const auto required_section_color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); + const auto scale = autoware_universe_utils::createMarkerScale(scale_x, scale_y, scale_z); + const auto desired_section_color = + autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); + const auto required_section_color = + autoware_universe_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); // intersection turn signal info { const auto & turn_signal_info = debug_data.intersection_turn_signal_info; - auto desired_start_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_start_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_desired_start", 0L, Marker::SPHERE, scale, desired_section_color); - auto desired_end_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_end_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_desired_end", 0L, Marker::SPHERE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = tier4_autoware_utils::createDefaultMarker( + auto required_start_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_required_start", 0L, Marker::SPHERE, scale, required_section_color); - auto required_end_marker = tier4_autoware_utils::createDefaultMarker( + auto required_end_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_required_end", 0L, Marker::SPHERE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -601,19 +604,19 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb { const auto & turn_signal_info = debug_data.behavior_turn_signal_info; - auto desired_start_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_start_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_desired_start", 0L, Marker::CUBE, scale, desired_section_color); - auto desired_end_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_end_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_desired_end", 0L, Marker::CUBE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = tier4_autoware_utils::createDefaultMarker( + auto required_start_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_required_start", 0L, Marker::CUBE, scale, required_section_color); - auto required_end_marker = tier4_autoware_utils::createDefaultMarker( + auto required_end_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_required_end", 0L, Marker::CUBE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -639,18 +642,18 @@ void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) constexpr double color_a = 0.999; const auto current_time = path.header.stamp; - auto left_marker = tier4_autoware_utils::createDefaultMarker( + auto left_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "left_bound", 0L, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_universe_utils::createMarkerScale(scale_x, scale_y, scale_z), + autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto lb : path.left_bound) { left_marker.points.push_back(lb); } - auto right_marker = tier4_autoware_utils::createDefaultMarker( + auto right_marker = autoware_universe_utils::createDefaultMarker( "map", current_time, "right_bound", 0L, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_universe_utils::createMarkerScale(scale_x, scale_y, scale_z), + autoware_universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto rb : path.right_bound) { right_marker.points.push_back(rb); } @@ -827,7 +830,7 @@ void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPt SetParametersResult BehaviorPathPlannerNode::onSetParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; rcl_interfaces::msg::SetParametersResult result; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index f925fd24d5691..61c7d45b22c78 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -466,10 +466,10 @@ void PlannerManager::publishDebugRootReferencePath( { using visualization_msgs::msg::Marker; MarkerArray array; - Marker m = tier4_autoware_utils::createDefaultMarker( + Marker m = autoware_universe_utils::createDefaultMarker( "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); + autoware_universe_utils::createMarkerScale(1.0, 1.0, 1.0), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position); array.markers.push_back(m); m.points.clear(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 4fd9f6de4727f..0240f4af7e4ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -102,7 +102,7 @@ struct DrivableAreaInfo struct Obstacle { geometry_msgs::msg::Pose pose; - tier4_autoware_utils::Polygon2d poly; + autoware_universe_utils::Polygon2d poly; bool is_left{true}; }; std::vector drivable_lanes{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 911d26ff19742..1e145b8983549 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -25,14 +25,14 @@ #include #include #include +#include +#include +#include #include #include #include #include #include -#include -#include -#include #include #include @@ -61,9 +61,9 @@ using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInt using autoware::rtc_interface::RTCInterface; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::generateUUID; using steering_factor_interface::SteeringFactorInterface; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 9cd0dabbcc765..78befd1451a0b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -17,7 +17,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include #include @@ -36,10 +36,10 @@ namespace autoware::behavior_path_planner { +using autoware_universe_utils::toHexString; using motion_utils::createDeadLineVirtualWallMarker; using motion_utils::createSlowDownVirtualWallMarker; using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -111,7 +111,7 @@ class SceneModuleManagerInterface void publishVirtualWall() const { - using tier4_autoware_utils::appendMarkerArray; + using autoware_universe_utils::appendMarkerArray; MarkerArray markers{}; @@ -155,7 +155,7 @@ class SceneModuleManagerInterface void publishMarker() const { - using tier4_autoware_utils::appendMarkerArray; + using autoware_universe_utils::appendMarkerArray; MarkerArray info_markers{}; MarkerArray debug_markers{}; @@ -271,7 +271,7 @@ class SceneModuleManagerInterface protected: void initInterface(rclcpp::Node * node, const std::vector & rtc_types) { - using tier4_autoware_utils::getOrDeclareParameter; + using autoware_universe_utils::getOrDeclareParameter; // init manager configuration { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp index b7372577b5dc2..4151f4dcacf19 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "std_msgs/msg/detail/color_rgba__struct.hpp" @@ -26,62 +26,62 @@ using std_msgs::msg::ColorRGBA; inline ColorRGBA red(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 0., 0., a); + return autoware_universe_utils::createMarkerColor(1., 0., 0., a); } inline ColorRGBA green(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0., 1., 0., a); + return autoware_universe_utils::createMarkerColor(0., 1., 0., a); } inline ColorRGBA blue(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0., 0., 1., a); + return autoware_universe_utils::createMarkerColor(0., 0., 1., a); } inline ColorRGBA yellow(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 1., 0., a); + return autoware_universe_utils::createMarkerColor(1., 1., 0., a); } inline ColorRGBA aqua(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0., 1., 1., a); + return autoware_universe_utils::createMarkerColor(0., 1., 1., a); } inline ColorRGBA magenta(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 0., 1., a); + return autoware_universe_utils::createMarkerColor(1., 0., 1., a); } inline ColorRGBA medium_orchid(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0.729, 0.333, 0.827, a); + return autoware_universe_utils::createMarkerColor(0.729, 0.333, 0.827, a); } inline ColorRGBA light_pink(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 0.713, 0.756, a); + return autoware_universe_utils::createMarkerColor(1., 0.713, 0.756, a); } inline ColorRGBA light_yellow(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 1., 0.878, a); + return autoware_universe_utils::createMarkerColor(1., 1., 0.878, a); } inline ColorRGBA light_steel_blue(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0.690, 0.768, 0.870, a); + return autoware_universe_utils::createMarkerColor(0.690, 0.768, 0.870, a); } inline ColorRGBA white(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 1., 1., a); + return autoware_universe_utils::createMarkerColor(1., 1., 1., a); } inline ColorRGBA grey(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(.5, .5, .5, a); + return autoware_universe_utils::createMarkerColor(.5, .5, .5, a); } inline std::vector colors_list(float a = 0.99) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp index 202893edb175b..fc0efc38ae617 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp @@ -42,13 +42,13 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredi using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index 13ef14c04b1b6..fa3236d7fd868 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include #include #include +#include +#include #include -#include -#include #include #include @@ -239,9 +239,9 @@ class TurnSignalDecider const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) const { + using autoware_universe_utils::pose2transform; + using autoware_universe_utils::transformVector; using boost::geometry::intersects; - using tier4_autoware_utils::pose2transform; - using tier4_autoware_utils::transformVector; const auto footprint = vehicle_info.createFootprint(); const auto start_itr = std::next(path.path.points.begin(), shift_line.start_idx); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index b826380e8eb72..c0f953578a213 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -18,7 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index 0b818c18be2fc..de29886c27b4e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include @@ -36,13 +36,13 @@ using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::MultiLineString2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::MultiPolygon2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::Segment2d; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::MultiLineString2d; +using autoware_universe_utils::MultiPoint2d; +using autoware_universe_utils::MultiPolygon2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using autoware_universe_utils::Segment2d; using SegmentRtree = boost::geometry::index::rtree>; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 5841935c59da0..fa04ff04c8e44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 3125324694588..8dfc590230064 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -70,7 +70,7 @@ bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::Cons bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); bool isPolygonOverlapLanelet( - const PredictedObject & object, const tier4_autoware_utils::Polygon2d & lanelet_polygon); + const PredictedObject & object, const autoware_universe_utils::Polygon2d & lanelet_polygon); bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index d713b00dd0299..421c495555c3c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT -#include +#include #include #include @@ -32,9 +32,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware_perception_msgs::msg::PredictedObject; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::Polygon2d; struct PoseWithVelocity { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 9d4f45f6fd982..92b7bab5c26ba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -18,8 +18,8 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include +#include +#include #include #include @@ -36,11 +36,11 @@ using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; +using autoware_universe_utils::calcYawDeviation; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 631a2e3e3b9cd..1e31bafe3fa0b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#include #include #include -#include #include #include @@ -29,9 +29,9 @@ #include namespace autoware::behavior_path_planner { +using autoware_universe_utils::generateUUID; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index ca0466b72389d..b8acf91724c6f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -19,7 +19,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include @@ -47,11 +47,11 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using autoware::route_handler::RouteHandler; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -145,7 +145,7 @@ bool checkCollisionWithExtraStoppingMargin( * @return Has collision or not */ bool checkCollisionBetweenPathFootprintsAndObjects( - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, + const autoware_universe_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin); /** @@ -153,7 +153,7 @@ bool checkCollisionBetweenPathFootprintsAndObjects( * @return Has collision or not */ bool checkCollisionBetweenFootprintAndObjects( - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, + const autoware_universe_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin); /** diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 6b20027610951..2a7b595db6b96 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -50,6 +50,7 @@ autoware_planning_msgs autoware_route_handler autoware_rtc_interface + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation @@ -59,7 +60,6 @@ object_recognition_utils rclcpp tf2 - tier4_autoware_utils tier4_planning_msgs traffic_light_utils visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 799f7f86b1589..154ceb6dc6dd5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -18,9 +18,9 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -31,13 +31,13 @@ namespace marker_utils { using autoware::behavior_path_planner::utils::calcPathArcLengthArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; void addFootprintMarker( @@ -49,13 +49,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware_universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -214,7 +214,7 @@ MarkerArray createShiftLengthMarkerArray( Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.9)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); marker.points.reserve(shift_distance.size()); for (size_t i = 0; i < shift_distance.size(); ++i) { @@ -271,7 +271,7 @@ MarkerArray createLaneletsAreaMarkerArray( "map", current_time, ns, static_cast(lanelet.id()), Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); if (!lanelet.polygon3d().empty()) { marker.points.reserve(lanelet.polygon3d().size() + 1); @@ -299,7 +299,7 @@ MarkerArray createPolygonMarkerArray( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast(lane_id), Marker::LINE_STRIP, createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); if (!polygon.points.empty()) { marker.points.reserve(polygon.points.size() + 1); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index c8a6d01dce3e8..f379d92afecf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -16,15 +16,15 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include #include #include @@ -577,8 +577,8 @@ geometry_msgs::msg::Pose TurnSignalDecider::get_required_end_point( const double terminal_yaw = tf2::getYaw(resampled_centerline.back().orientation); for (size_t i = 0; i < resampled_centerline.size(); ++i) { const double yaw = tf2::getYaw(resampled_centerline.at(i).orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw - terminal_yaw); - if (std::fabs(yaw_diff) < tier4_autoware_utils::deg2rad(intersection_angle_threshold_deg_)) { + const double yaw_diff = autoware_universe_utils::normalizeRadian(yaw - terminal_yaw); + if (std::fabs(yaw_diff) < autoware_universe_utils::deg2rad(intersection_angle_threshold_deg_)) { return resampled_centerline.at(i); } } @@ -634,9 +634,9 @@ void TurnSignalDecider::initialize_intersection_info() geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation( const Point & src_point, const Point & dst_point) { - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - return tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + const double pitch = autoware_universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware_universe_utils::calcAzimuthAngle(src_point, dst_point); + return autoware_universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( @@ -647,7 +647,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { - using tier4_autoware_utils::getPose; + using autoware_universe_utils::getPose; const auto & p = parameters; const auto & rh = route_handler; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 1dfc217d1cb39..32cced71eec74 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -20,11 +20,11 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include #include #include #include #include -#include #include @@ -50,8 +50,8 @@ void reuse_previous_poses( std::vector cropped_curvatures; const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( prev_poses, 0, ego_point) < 0.0; - const auto ego_is_far = !prev_poses.empty() && - tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; + const auto ego_is_far = !prev_poses.empty() && autoware_universe_utils::calcDistance2d( + ego_point, prev_poses.front()) < 0.0; // make sure the reused points are not behind the current original drivable area LineString2d left_bound; LineString2d right_bound; @@ -161,7 +161,7 @@ void apply_arc_length_range_smoothing( auto arc_length = boost::geometry::distance( bound_projections[path_idx].point, convert_point(bound[bound_idx + 1])); const auto update_arc_length_and_bound_expansions = [&](auto idx) { - arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + arc_length += autoware_universe_utils::calcDistance2d(bound[idx - 1], bound[idx]); bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]); }; for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { @@ -200,7 +200,7 @@ void apply_bound_change_rate_limit( if (distances.empty()) return; const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { if (exp[from] > exp[to]) { - const auto arc_length = tier4_autoware_utils::calcDistance2d(bound[from], bound[to]); + const auto arc_length = autoware_universe_utils::calcDistance2d(bound[from], bound[to]); const auto smoothed_dist = exp[from] - arc_length * max_rate; exp[to] = std::max(exp[to], smoothed_dist); } @@ -284,12 +284,12 @@ void expand_bound( for (auto idx = 1LU; idx < bound.size(); ++idx) { bool is_intersecting = false; for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { - const auto intersection = tier4_autoware_utils::intersect( + const auto intersection = autoware_universe_utils::intersect( bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); if ( intersection && - tier4_autoware_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && - tier4_autoware_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + autoware_universe_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + autoware_universe_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { idx = succ_idx; is_intersecting = true; } @@ -363,7 +363,7 @@ void expand_drivable_area( { // skip if no bounds or not enough points to calculate path curvature if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); const auto & params = planner_data->drivable_area_expansion_parameters; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index e465788c54b8a..b01669871276a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -16,8 +16,8 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include -#include +#include +#include #include @@ -37,7 +37,8 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 { const auto angle = tf2::getYaw(pose.orientation); return translate_polygon( - tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); + autoware_universe_utils::rotatePolygon(base_footprint, angle), pose.position.x, + pose.position.y); } MultiPolygon2d create_object_footprints( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 69bedfd52a372..be7a5cac7f0a0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -16,12 +16,12 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include +#include #include #include #include #include -#include -#include #include @@ -55,8 +55,8 @@ std::vector removeSharpPoints(const std::vector & points) const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware_universe_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = autoware_universe_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; @@ -82,9 +82,9 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::normalizeRadian; + using autoware_universe_utils::calcAzimuthAngle; + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::normalizeRadian; std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); @@ -119,9 +119,9 @@ size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Pose & target_point, const double yaw_threshold) { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::normalizeRadian; + using autoware_universe_utils::calcAzimuthAngle; + using autoware_universe_utils::calcDistance2d; + using autoware_universe_utils::normalizeRadian; std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); @@ -245,7 +245,7 @@ std::optional> intersectBound( static_cast(bound.size()) - 1, static_cast(std::max(seg_idx1, seg_idx2)) + 1 + 5)); for (int i = start_idx; i < static_cast(end_idx); ++i) { const auto intersect_point = - tier4_autoware_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); + autoware_universe_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); if (intersect_point) { std::pair result; result.first = static_cast(i); @@ -261,7 +261,7 @@ double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { - using tier4_autoware_utils::calcSquaredDistance2d; + using autoware_universe_utils::calcSquaredDistance2d; const auto & a = segment_start_point; const auto & b = segment_end_point; @@ -437,7 +437,7 @@ std::vector concatenateTwoPolygons( double min_dist_to_intersection = std::numeric_limits::max(); PolygonPoint closest_intersect_point; for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { - const auto intersection = tier4_autoware_utils::intersect( + const auto intersection = autoware_universe_utils::intersect( get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, get_in_poly().at(i).point, get_in_poly().at(i + 1).point); if (!intersection) { @@ -454,7 +454,7 @@ std::vector concatenateTwoPolygons( const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0}; const double dist_to_intersection = - tier4_autoware_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); + autoware_universe_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); if (dist_to_intersection < min_dist_to_intersection) { closest_idx = i; min_dist_to_intersection = dist_to_intersection; @@ -640,7 +640,7 @@ std::vector updateBoundary( namespace autoware::behavior_path_planner::utils { -using tier4_autoware_utils::Point2d; +using autoware_universe_utils::Point2d; std::optional getOverlappedLaneletId(const std::vector & lanes) { @@ -852,7 +852,7 @@ void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, const bool is_driving_forward) { - using tier4_autoware_utils::calcOffsetPose; + using autoware_universe_utils::calcOffsetPose; // remove path points which is close to the previous point PathWithLaneId resampled_path{}; @@ -873,7 +873,7 @@ void generateDrivableArea( // add last point of path if enough far from the one of resampled path constexpr double th_last_point_distance = 0.3; if ( - tier4_autoware_utils::calcDistance2d( + autoware_universe_utils::calcDistance2d( resampled_path.points.back().point.pose.position, path.points.back().point.pose.position) > th_last_point_distance) { resampled_path.points.push_back(path.points.back()); @@ -946,7 +946,7 @@ void generateDrivableArea( p_line.push_back(p2); bool intersection_found = false; for (size_t j = i + 2; j < bound.size() - 1; j++) { - const double distance = tier4_autoware_utils::calcDistance2d(bound.at(i), bound.at(j)); + const double distance = autoware_universe_utils::calcDistance2d(bound.at(i), bound.at(j)); if (distance > intersection_check_distance) { break; } @@ -1045,7 +1045,7 @@ void extractObstaclesFromDrivableArea( std::vector edge_points; for (int i = 0; i < static_cast(obstacle.poly.outer().size()) - 1; ++i) { // NOTE: There is a duplicated points - edge_points.push_back(tier4_autoware_utils::createPoint( + edge_points.push_back(autoware_universe_utils::createPoint( obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(), path.points.at(nearest_path_idx).point.pose.position.z)); } @@ -1302,12 +1302,12 @@ std::pair, bool> getBoundWithFreeSpaceAreas( const std::vector & other_side_bound, const std::shared_ptr planner_data, const bool is_left) { + using autoware_universe_utils::getPose; + using autoware_universe_utils::pose2transform; + using autoware_universe_utils::transformVector; using lanelet::utils::to2D; using lanelet::utils::conversion::toGeomMsgPt; using lanelet::utils::conversion::toLaneletPoint; - using tier4_autoware_utils::getPose; - using tier4_autoware_utils::pose2transform; - using tier4_autoware_utils::transformVector; const auto & route_handler = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; @@ -1382,12 +1382,12 @@ std::pair, bool> getBoundWithFreeSpaceAreas( return bound; } - const auto p_offset = tier4_autoware_utils::calcOffsetPose( + const auto p_offset = autoware_universe_utils::calcOffsetPose( ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); std::vector ret; for (size_t i = 1; i < bound.size(); ++i) { - const auto intersect = tier4_autoware_utils::intersect( + const auto intersect = autoware_universe_utils::intersect( ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), toGeomMsgPt(bound.at(i))); @@ -1492,7 +1492,7 @@ std::vector postProcess( const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); if (bound.empty()) { bound.push_back(cp); - } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + } else if (autoware_universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { bound.push_back(cp); } } @@ -1596,7 +1596,7 @@ std::vector postProcess( // Insert middle points for (size_t i = start_idx + 1; i <= goal_idx; ++i) { const auto & next_point = tmp_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); + const double dist = autoware_universe_utils::calcDistance2d(processed_bound.back(), next_point); if (dist > overlap_threshold) { processed_bound.push_back(next_point); } @@ -1604,7 +1604,8 @@ std::vector postProcess( // Insert a goal point if ( - tier4_autoware_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { + autoware_universe_utils::calcDistance2d(processed_bound.back(), goal_point) > + overlap_threshold) { processed_bound.push_back(goal_point); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 0ff327e187aba..875de7ff18400 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -14,16 +14,16 @@ #include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include -#include +#include +#include #include namespace autoware::behavior_path_planner { -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::transformPose; +using autoware_universe_utils::createQuaternionFromYaw; +using autoware_universe_utils::normalizeRadian; +using autoware_universe_utils::transformPose; int discretizeAngle(const double theta, const int theta_size) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 03df52efad03e..b7c468ebd3d83 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -31,14 +31,14 @@ #include #include +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::inverseTransformPoint; +using autoware_universe_utils::normalizeRadian; +using autoware_universe_utils::transformPose; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPoint; -using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::transformPose; using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner @@ -132,10 +132,10 @@ std::vector GeometricParallelParking::generatePullOverPaths( // check the continuity of straight path and arc path const Pose & road_path_last_pose = straight_path.points.back().point.pose; const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; - const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian( tf2::getYaw(road_path_last_pose.orientation) - tf2::getYaw(arc_path_first_pose.orientation))); const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); - if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + if (yaw_diff > autoware_universe_utils::deg2rad(5.0) || distance > 0.1) { return std::vector{}; } @@ -282,10 +282,10 @@ bool GeometricParallelParking::planPullOut( // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; - const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian( tf2::getYaw(road_path_first_pose.orientation) - tf2::getYaw(arc_path_last_pose.orientation))); const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose); - if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + if (yaw_diff > autoware_universe_utils::deg2rad(5.0) || distance > 0.1) { continue; } @@ -399,7 +399,7 @@ std::vector GeometricParallelParking::planOneTrial( // combine road and shoulder lanes // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane lanelet::ConstLanelets lanes{}; - tier4_autoware_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); + autoware_universe_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); for (const auto & lane : road_lanes) { if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) { lanes.push_back(lane); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 0770ccc514086..0514680f49d94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include #include -#include #include @@ -71,16 +71,16 @@ bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::Cons } bool isPolygonOverlapLanelet( - const PredictedObject & object, const tier4_autoware_utils::Polygon2d & lanelet_polygon) + const PredictedObject & object, const autoware_universe_utils::Polygon2d & lanelet_polygon) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } @@ -315,7 +315,7 @@ ExtendedPredictedObject transform( for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths[i].path.emplace_back( t, *obj_pose, obj_velocity, obj_polygon); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 4f47aa8964f0f..4b3910e351f86 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -15,10 +15,10 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/ros/uuid_helper.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include #include @@ -55,13 +55,13 @@ bool isTargetObjectFront( { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_offset_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + autoware_universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & obj_edge : obj_polygon_outer) { - const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); - if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + const auto obj_point = autoware_universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (autoware_universe_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { return true; } } @@ -75,12 +75,12 @@ bool isTargetObjectFront( { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_point = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; + autoware_universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & obj_edge : obj_polygon_outer) { - const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + const auto obj_point = autoware_universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; } @@ -111,13 +111,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -125,16 +125,16 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return tier4_autoware_utils::isClockwise(polygon) + return autoware_universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware_universe_utils::inverseClockwise(polygon); } Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { return obj_polygon; } @@ -145,8 +145,8 @@ Polygon2d createExtendedPolygon( double min_y = std::numeric_limits::max(); const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & polygon_p : obj_polygon_outer) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose); + const auto obj_p = autoware_universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto transformed_p = autoware_universe_utils::inverseTransformPoint(obj_p, obj_pose); max_x = std::max(transformed_p.x, max_x); min_x = std::min(transformed_p.x, min_x); @@ -168,13 +168,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -182,9 +182,9 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return tier4_autoware_utils::isClockwise(polygon) + return autoware_universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware_universe_utils::inverseClockwise(polygon); } Polygon2d createExtendedPolygonAlongPath( @@ -224,49 +224,49 @@ Polygon2d createExtendedPolygonAlongPath( { const auto p_offset = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = start_idx + 1; i < end_idx + 1; ++i) { - const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); - const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); + const auto p = autoware_universe_utils::getPose(planned_path.points.at(i)); + const auto p_offset = autoware_universe_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = - tier4_autoware_utils::calcOffsetPose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0); + const auto p_offset = autoware_universe_utils::calcOffsetPose( + lon_offset_pose.value(), base_to_front, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = tier4_autoware_utils::calcOffsetPose( + const auto p_offset = autoware_universe_utils::calcOffsetPose( lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = end_idx; i > start_idx; --i) { - const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); - const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); + const auto p = autoware_universe_utils::getPose(planned_path.points.at(i)); + const auto p_offset = autoware_universe_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + const auto p_offset = autoware_universe_utils::calcOffsetPose( + base_link_pose, backward_lon_offset, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { const auto p_offset = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + autoware_universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } - return tier4_autoware_utils::isClockwise(polygon) + return autoware_universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware_universe_utils::inverseClockwise(polygon); } std::vector createExtendedPolygonsFromPoseWithVelocityStamped( @@ -283,7 +283,7 @@ std::vector createExtendedPolygonsFromPoseWithVelocityStamped( const double width = vehicle_info.vehicle_width_m + lat_margin * 2; const auto polygon = - tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width); + autoware_universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); polygons.push_back(polygon); } @@ -350,7 +350,7 @@ std::optional calcInterpolatedPoseWithVelocity( const double time_step = pt.time - prev_pt.time; const double ratio = std::clamp(offset / time_step, 0.0, 1.0); const auto interpolated_pose = - tier4_autoware_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); + autoware_universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); const double interpolated_velocity = interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); return PoseWithVelocityStamped{relative_time, interpolated_pose, interpolated_velocity}; @@ -378,7 +378,7 @@ std::optional getInterpolatedPoseWithVelocity const auto & velocity = interpolation_result->velocity; const auto ego_polygon = - tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width); + autoware_universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon}; } @@ -406,7 +406,7 @@ std::optional getInterpolatedPoseWithVelocity const auto & pose = interpolation_result->pose; const auto & velocity = interpolation_result->velocity; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(pose, shape); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(pose, shape); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, obj_polygon}; } @@ -691,10 +691,11 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; debug.current_obj_pose = obj.initial_pose.pose; - debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + debug.extended_obj_polygon = + autoware_universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); debug.obj_shape = obj.shape; debug.current_twist = obj.initial_twist.twist; - return {tier4_autoware_utils::toBoostUUID(obj.uuid), debug}; + return {autoware_universe_utils::toBoostUUID(obj.uuid), debug}; } void updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 872cc69d6bde9..2e79e54499c3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -17,12 +17,12 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include #include #include -#include #include @@ -50,7 +50,7 @@ std::vector calcPathArcLengthArray( for (size_t i = bounded_start + 1; i < bounded_end; ++i) { sum += - tier4_autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); + autoware_universe_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); out.push_back(sum); } return out; @@ -162,7 +162,7 @@ size_t getIdxByArclength( throw std::runtime_error("[getIdxByArclength] path points must be > 0"); } - using tier4_autoware_utils::calcDistance2d; + using autoware_universe_utils::calcDistance2d; double sum_length = 0.0; if (signed_arc >= 0.0) { for (size_t i = target_idx; i < path.points.size() - 1; ++i) { @@ -471,7 +471,7 @@ std::vector interpolatePose( std::vector interpolated_poses{}; // output const std::vector base_s{ - 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + 0, autoware_universe_utils::calcDistance2d(start_pose.position, end_pose.position)}; const std::vector base_x{start_pose.position.x, end_pose.position.x}; const std::vector base_y{start_pose.position.y, end_pose.position.y}; std::vector new_s; @@ -489,7 +489,7 @@ std::vector interpolatePose( std::sin(tf2::getYaw(end_pose.orientation)), new_s); for (size_t i = 0; i < interpolated_x.size(); ++i) { Pose pose{}; - pose = tier4_autoware_utils::calcInterpolatedPose( + pose = autoware_universe_utils::calcInterpolatedPose( end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); pose.position.x = interpolated_x.at(i); pose.position.y = interpolated_y.at(i); @@ -513,7 +513,7 @@ Pose getUnshiftedEgoPose(const Pose & ego_pose, const ShiftedPath & prev_path) // ego_pose. auto unshifted_pose = motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; - unshifted_pose = tier4_autoware_utils::calcOffsetPose( + unshifted_pose = autoware_universe_utils::calcOffsetPose( unshifted_pose, 0.0, -prev_path.shift_length.at(closest_idx), 0.0); unshifted_pose.orientation = ego_pose.orientation; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 49f7030456914..d92cdd192de39 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -98,7 +98,8 @@ std::optional calcDistanceToRedTrafficLight( const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); - return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z)); + return calcSignedArcLength( + path.points, ego_pos, autoware_universe_utils::createPoint(x, y, z)); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index e08ee107b14ba..e81a928d1281a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -17,13 +17,13 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include +#include +#include #include #include #include #include -#include -#include -#include #include @@ -73,9 +73,9 @@ namespace autoware::behavior_path_planner::utils { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::Shape; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::Point2d; using geometry_msgs::msg::PoseWithCovarianceStamped; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; std::optional getPolygonByPoint( const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, @@ -111,7 +111,7 @@ double getDistanceBetweenPredictedPaths( if (!ego_pose) { continue; } - double distance = tier4_autoware_utils::calcDistance3d(*object_pose, *ego_pose); + double distance = autoware_universe_utils::calcDistance3d(*object_pose, *ego_pose); if (distance < min_distance) { min_distance = distance; } @@ -128,7 +128,7 @@ double getDistanceBetweenPredictedPathAndObject( double min_distance = std::numeric_limits::max(); rclcpp::Time ros_start_time = clock.now() + rclcpp::Duration::from_seconds(start_time); rclcpp::Time ros_end_time = clock.now() + rclcpp::Duration::from_seconds(end_time); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); for (double t = start_time; t < end_time; t += resolution) { const auto ego_pose = object_recognition_utils::calcInterpolatedPose(ego_path, t); if (!ego_pose) { @@ -145,7 +145,7 @@ double getDistanceBetweenPredictedPathAndObject( } bool checkCollisionBetweenPathFootprintsAndObjects( - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, + const autoware_universe_utils::LinearRing2d & local_vehicle_footprint, const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin) { for (const auto & p : ego_path.points) { @@ -158,14 +158,14 @@ bool checkCollisionBetweenPathFootprintsAndObjects( } bool checkCollisionBetweenFootprintAndObjects( - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const autoware_universe_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin) { const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); + transformVector(local_vehicle_footprint, autoware_universe_utils::pose2transform(ego_pose)); for (const auto & object : dynamic_objects.objects) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); if (distance < margin) return true; } @@ -176,18 +176,18 @@ double calcLateralDistanceFromEgoToObject( const Pose & ego_pose, const double vehicle_width, const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(dynamic_object); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(dynamic_object); const auto vehicle_left_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); + autoware_universe_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); const auto vehicle_right_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); + autoware_universe_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); const double signed_distance_from_left = - tier4_autoware_utils::calcLateralDeviation(vehicle_left_pose, point); + autoware_universe_utils::calcLateralDeviation(vehicle_left_pose, point); const double signed_distance_from_right = - tier4_autoware_utils::calcLateralDeviation(vehicle_right_pose, point); + autoware_universe_utils::calcLateralDeviation(vehicle_right_pose, point); if (signed_distance_from_left < 0.0 && signed_distance_from_right > 0.0) { // point is between left and right @@ -206,21 +206,21 @@ double calcLongitudinalDistanceFromEgoToObject( const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(dynamic_object); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(dynamic_object); const auto vehicle_front_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); + autoware_universe_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); const auto vehicle_rear_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); + autoware_universe_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); // forward is positive const double signed_distance_from_front = - tier4_autoware_utils::calcLongitudinalDeviation(vehicle_front_pose, point); + autoware_universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point); // backward is positive const double signed_distance_from_rear = - -tier4_autoware_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); + -autoware_universe_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); if (signed_distance_from_front < 0.0 && signed_distance_from_rear < 0.0) { // point is between front and rear @@ -252,7 +252,7 @@ std::vector calcObjectsDistanceToPath( { std::vector distance_array; for (const auto & obj : objects.objects) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(obj); LineString2d ego_path_line; ego_path_line.reserve(ego_path.points.size()); for (const auto & p : ego_path.points) { @@ -288,7 +288,7 @@ std::optional findIndexOutOfGoalSearchRange( const auto & lane_ids = points.at(i).lane_ids; const double dist_to_goal = - tier4_autoware_utils::calcDistance2d(points.at(i).point.pose, goal); + autoware_universe_utils::calcDistance2d(points.at(i).point.pose, goal); const bool is_goal_lane_id_in_point = std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { @@ -305,7 +305,7 @@ std::optional findIndexOutOfGoalSearchRange( // find index out of goal search range size_t min_dist_out_of_range_index = min_dist_index; for (int i = min_dist_index; 0 <= i; --i) { - const double dist = tier4_autoware_utils::calcDistance2d(points.at(i).point, goal); + const double dist = autoware_universe_utils::calcDistance2d(points.at(i).point, goal); min_dist_out_of_range_index = i; if (max_dist < dist) { break; @@ -341,7 +341,7 @@ bool setGoal( PathPointWithLaneId pre_refined_goal{}; constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = - tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); + autoware_universe_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); const size_t closest_seg_idx_for_pre_goal = findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = @@ -482,7 +482,7 @@ bool isInLaneletWithYawThreshold( const double pose_yaw = tf2::getYaw(current_pose.orientation); const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position); const double angle_diff = - std::abs(tier4_autoware_utils::normalizeRadian(lanelet_angle - pose_yaw)); + std::abs(autoware_universe_utils::normalizeRadian(lanelet_angle - pose_yaw)); return (angle_diff < std::abs(yaw_threshold)) && lanelet::utils::isInLanelet(current_pose, lanelet, radius); @@ -509,7 +509,7 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - const double yaw_threshold = tier4_autoware_utils::deg2rad(90); + const double yaw_threshold = autoware_universe_utils::deg2rad(90); if ( closest_road_lane.id() == goal_lane.id() && isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { @@ -562,8 +562,8 @@ bool isEgoWithinOriginalLane( const auto base_link2front = common_param.base_link2front; const auto base_link2rear = common_param.base_link2rear; const auto vehicle_width = common_param.vehicle_width; - const auto vehicle_poly = - tier4_autoware_utils::toFootprint(current_pose, base_link2front, base_link2rear, vehicle_width); + const auto vehicle_poly = autoware_universe_utils::toFootprint( + current_pose, base_link2front, base_link2rear, vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. for (const auto & p : vehicle_poly.outer()) { @@ -849,16 +849,16 @@ std::optional getSignedDistanceFromBoundary( rear_left.y = vehicle_width / 2; front_left.x = base_link2front; front_left.y = vehicle_width / 2; - rear_corner_point = tier4_autoware_utils::transformPoint(rear_left, vehicle_pose); - front_corner_point = tier4_autoware_utils::transformPoint(front_left, vehicle_pose); + rear_corner_point = autoware_universe_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = autoware_universe_utils::transformPoint(front_left, vehicle_pose); } else { Point front_right, rear_right; rear_right.x = -base_link2rear; rear_right.y = -vehicle_width / 2; front_right.x = base_link2front; front_right.y = -vehicle_width / 2; - rear_corner_point = tier4_autoware_utils::transformPoint(rear_right, vehicle_pose); - front_corner_point = tier4_autoware_utils::transformPoint(front_right, vehicle_pose); + rear_corner_point = autoware_universe_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = autoware_universe_utils::transformPoint(front_right, vehicle_pose); } const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); @@ -882,16 +882,18 @@ std::optional getSignedDistanceFromBoundary( const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); - const Point inverse_p1 = tier4_autoware_utils::inverseTransformPoint(p1, vehicle_corner_pose); - const Point inverse_p2 = tier4_autoware_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const Point inverse_p1 = + autoware_universe_utils::inverseTransformPoint(p1, vehicle_corner_pose); + const Point inverse_p2 = + autoware_universe_utils::inverseTransformPoint(p2, vehicle_corner_pose); const double dx_p1 = inverse_p1.x; const double dx_p2 = inverse_p2.x; const double dy_p1 = inverse_p1.y; const double dy_p2 = inverse_p2.y; // Calculate the Euclidean distances between vehicle's corner and the current and next points. - const double distance1 = tier4_autoware_utils::calcDistance2d(p1, vehicle_corner_point); - const double distance2 = tier4_autoware_utils::calcDistance2d(p2, vehicle_corner_point); + const double distance1 = autoware_universe_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = autoware_universe_utils::calcDistance2d(p2, vehicle_corner_point); // If one of the bound points is behind and the other is in front of the vehicle corner point // and any of these points is closer than the current minimum distance, @@ -944,9 +946,9 @@ std::optional getSignedDistanceFromBoundary( bound_pose.orientation = vehicle_pose.orientation; const Point inverse_rear_point = - tier4_autoware_utils::inverseTransformPoint(rear_corner_point, bound_pose); + autoware_universe_utils::inverseTransformPoint(rear_corner_point, bound_pose); const Point inverse_front_point = - tier4_autoware_utils::inverseTransformPoint(front_corner_point, bound_pose); + autoware_universe_utils::inverseTransformPoint(front_corner_point, bound_pose); const double dx_rear = inverse_rear_point.x; const double dx_front = inverse_front_point.x; const double dy_rear = inverse_rear_point.y; @@ -1004,9 +1006,9 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) } polygon.outer().push_back(polygon.outer().front()); - return tier4_autoware_utils::isClockwise(polygon) + return autoware_universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware_universe_utils::inverseClockwise(polygon); } Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) @@ -1017,7 +1019,8 @@ Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) } ret.outer().push_back(ret.outer().front()); - return tier4_autoware_utils::isClockwise(ret) ? ret : tier4_autoware_utils::inverseClockwise(ret); + return autoware_universe_utils::isClockwise(ret) ? ret + : autoware_universe_utils::inverseClockwise(ret); } std::vector getTargetLaneletPolygons( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 9d597648a130e..543fbecd1b38b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -16,7 +16,7 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include +#include #include @@ -29,11 +29,11 @@ constexpr double epsilon = 1e-6; using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using autoware_perception_msgs::msg::Shape; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { @@ -47,8 +47,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -75,8 +75,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = tier4_autoware_utils::createPoint(3.0, 4.0, 0.0); - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware_universe_utils::createPoint(3.0, 4.0, 0.0); + ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -103,9 +103,9 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.position = autoware_universe_utils::createPoint(0.0, 0.0, 0.0); ego_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::deg2rad(60)); + autoware_universe_utils::createQuaternionFromYaw(autoware_universe_utils::deg2rad(60)); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -134,13 +134,13 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { using autoware::behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternionFromYaw; + using autoware_universe_utils::createPoint; + using autoware_universe_utils::createQuaternionFromYaw; { Pose obj_pose; obj_pose.position = createPoint(0.0, 0.0, 0.0); - obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + obj_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(0.0); Shape shape; shape.type = autoware_perception_msgs::msg::Shape::POLYGON; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index 8ebcd3af7caf6..51e3a216c3927 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include "autoware_planning_msgs/msg/path_point.hpp" #include @@ -27,12 +27,12 @@ using autoware::behavior_path_planner::Pose; using autoware::behavior_path_planner::TurnSignalDecider; using autoware::behavior_path_planner::TurnSignalInfo; using autoware_planning_msgs::msg::PathPoint; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromYaw; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromYaw; using tier4_planning_msgs::msg::PathPointWithLaneId; constexpr double nearest_dist_threshold = 5.0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index d8e6ab9050080..fd71b7d171a41 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -23,6 +23,11 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "autoware/behavior_path_sampling_planner_module/util.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_bezier_sampler/bezier_sampling.hpp" #include "autoware_frenet_planner/frenet_planner.hpp" #include "autoware_sampler_common/constraints/footprint.hpp" @@ -35,11 +40,6 @@ #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" @@ -75,8 +75,8 @@ struct SamplingPlannerDebugData { std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; - std::vector obstacles{}; - std::vector footprints{}; + std::vector obstacles{}; + std::vector footprints{}; }; class SamplingPlannerModule : public SceneModuleInterface { @@ -207,7 +207,7 @@ class SamplingPlannerModule : public SceneModuleInterface motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose); if (!nearest_index) return false; auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double { - const auto rpy = tier4_autoware_utils::getRPY(quat); + const auto rpy = autoware_universe_utils::getRPY(quat); return rpy.z; }; const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index f4fc1ae34f3e1..15a9e585a7690 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -21,11 +21,11 @@ #include namespace autoware::behavior_path_planner { -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::MultiPolygon2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::MultiPoint2d; +using autoware_universe_utils::MultiPolygon2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; struct SamplingPlannerParameters { // constraints.hard diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp index ef5a2e0d7f97f..c43d1fe601b5f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp @@ -17,8 +17,8 @@ #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" -#include -#include +#include +#include #include @@ -101,7 +101,7 @@ inline autoware::sampler_common::State getInitialState( { autoware::sampler_common::State initial_state; Point2d initial_state_pose{pose.position.x, pose.position.y}; - const auto rpy = tier4_autoware_utils::getRPY(pose.orientation); + const auto rpy = autoware_universe_utils::getRPY(pose.orientation); initial_state.pose = initial_state_pose; initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y}); initial_state.heading = rpy.z; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index 7934c2fb0ae91..3b3c699f8f853 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -21,6 +21,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs @@ -36,7 +37,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp index 73741e08c61ee..6ffd3ff116284 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_sampling_planner_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -77,7 +77,7 @@ void SamplingPlannerModuleManager::init(rclcpp::Node * node) void SamplingPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto & p = parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 5bd4a5cc2eff4..87fd2156b0eb1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -16,14 +16,14 @@ namespace autoware::behavior_path_planner { +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::getPoint; +using autoware_universe_utils::Point2d; using geometry_msgs::msg::Point; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::getPoint; -using tier4_autoware_utils::Point2d; namespace bg = boost::geometry; namespace bgi = boost::geometry::index; @@ -103,7 +103,7 @@ SamplingPlannerModule::SamplingPlannerModule( // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = - // tier4_autoware_utils::getRPY(input_data.goal_pose.orientation).z; const auto & + // autoware_universe_utils::getRPY(input_data.goal_pose.orientation).z; const auto & // last_point_yaw = path.yaws.back(); const double angle_difference = std::abs(last_point_yaw // - goal_pose_yaw); return angle_difference / (3.141519 / 4.0); // }); @@ -365,9 +365,9 @@ void SamplingPlannerModule::prepareConstraints( size_t i = 0; for (const auto & o : predicted_objects->objects) { if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) { - const auto polygon = tier4_autoware_utils::toPolygon2d(o); + const auto polygon = autoware_universe_utils::toPolygon2d(o); constraints.obstacle_polygons.push_back(polygon); - const auto box = boost::geometry::return_envelope(polygon); + const auto box = boost::geometry::return_envelope(polygon); constraints.rtree.insert(std::make_pair(box, i)); } i++; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml index f3ac76d2bb1bb..e998bd7db83ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml @@ -21,11 +21,11 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_planning_msgs + autoware_universe_utils geometry_msgs motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp index eb590a3c81d86..ddc9c820a6667 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_side_shift_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -48,7 +48,7 @@ void SideShiftModuleManager::init(rclcpp::Node * node) void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; [[maybe_unused]] auto p = parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index a6706d9ab409b..7cb3ab04548ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -28,12 +28,12 @@ namespace autoware::behavior_path_planner { +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::getPoint; using geometry_msgs::msg::Point; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, @@ -195,7 +195,8 @@ void SideShiftModule::updateData() const auto longest_dist_to_shift_line = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start)); + max_dist = + std::max(max_dist, autoware_universe_utils::calcDistance2d(getEgoPose(), pnt.start)); } return max_dist; }(); @@ -467,7 +468,7 @@ void SideShiftModule::setDebugMarkersVisualization() const debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + autoware_universe_utils::appendMarkerArray(added, &debug_marker_); }; const auto add_shift_line_marker = [this, add]( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index a533765ff26fc..52675653e7b32 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -30,8 +30,8 @@ namespace autoware::behavior_path_planner { +using autoware_universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; class PullOutPlannerBase diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index 5b2ad9f8dc9c1..eda8328b07f60 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -23,10 +23,10 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_rtc_interface + autoware_universe_utils motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 916f01329d263..8be6bc911634a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -81,7 +81,7 @@ std::optional FreespacePullOut::plan( const size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware_universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); if (distance < th_end_distance) { last_path.points.erase(it, last_path.points.end()); break; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index f3ffaffc91e61..060286517a1b1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -18,14 +18,14 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index cd2cd911433de..0806978a7ef3f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_start_planner_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -351,7 +351,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto & p = parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index e59eec9070c13..23fa9535b5105 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -19,18 +19,18 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include #include +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 99af7b993b044..b53dc6861ea32 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -41,9 +41,9 @@ using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using autoware_universe_utils::calcOffsetPose; using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; -using tier4_autoware_utils::calcOffsetPose; // set as macro so that calling function name will be printed. // debug print is heavy. turn on only when debugging. @@ -274,7 +274,7 @@ bool StartPlannerModule::hasFinishedBackwardDriving() const // check ego car is close enough to pull out start pose and stopped const auto current_pose = planner_data_->self_odometry->pose.pose; const auto distance = - tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); + autoware_universe_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); const bool is_near = distance < parameters_->th_arrived_distance; const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); @@ -383,7 +383,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { const double x = boundary_point.x(); const double y = boundary_point.y(); - boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + boundary_path.push_back(autoware_universe_utils::createPoint(x, y, 0.0)); }); return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); @@ -396,7 +396,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); if (!start_pose_nearest_segment_index) return false; - const auto start_pose_point_msg = tier4_autoware_utils::createPoint( + const auto start_pose_point_msg = autoware_universe_utils::createPoint( start_pose.position.x, start_pose.position.y, start_pose.position.z); const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset( centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); @@ -412,8 +412,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const geometry_msgs::msg::Pose & ego_overhang_point_as_pose, const bool ego_is_merging_from_the_left) -> std::optional> { const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + const auto vehicle_footprint = transformVector( + local_vehicle_footprint, autoware_universe_utils::pose2transform(current_pose)); double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits::max(); @@ -524,7 +524,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - return tier4_autoware_utils::calcDistance2d( + return autoware_universe_utils::calcDistance2d( start_pose.position, planner_data_->self_odometry->pose.pose.position) > parameters_->th_arrived_distance; } @@ -532,7 +532,7 @@ bool StartPlannerModule::isCloseToOriginalStartPose() const bool StartPlannerModule::hasArrivedAtGoal() const { const Pose goal_pose = planner_data_->route_handler->getGoalPose(); - return tier4_autoware_utils::calcDistance2d( + return autoware_universe_utils::calcDistance2d( goal_pose.position, planner_data_->self_odometry->pose.pose.position) < parameters_->th_arrived_distance; } @@ -913,7 +913,7 @@ bool StartPlannerModule::findPullOutPath( // if start_pose_candidate is far from refined_start_pose, backward driving is necessary constexpr double epsilon = 0.01; const double backwards_distance = - tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose); + autoware_universe_utils::calcDistance2d(start_pose_candidate, refined_start_pose); const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); @@ -1240,7 +1240,7 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( bool StartPlannerModule::hasReachedFreespaceEnd() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; - return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + return autoware_universe_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < parameters_->th_arrived_distance; } @@ -1282,7 +1282,7 @@ bool StartPlannerModule::hasFinishedCurrentPath() const auto current_path = getCurrentPath(); const auto current_path_end = current_path.points.back(); const auto self_pose = planner_data_->self_odometry->pose.pose; - const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < + const bool is_near_target = autoware_universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; return is_near_target && isStopped(); @@ -1562,6 +1562,9 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() { + using autoware_universe_utils::createDefaultMarker; + using autoware_universe_utils::createMarkerColor; + using autoware_universe_utils::createMarkerScale; using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; @@ -1572,9 +1575,6 @@ void StartPlannerModule::setDebugData() using marker_utils::showPolygon; using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; using visualization_msgs::msg::Marker; const auto red_color = createMarkerColor(1.0, 0.0, 0.0, 0.999); @@ -1588,7 +1588,7 @@ void StartPlannerModule::setDebugData() for (auto & marker : added.markers) { marker.lifetime = life_time; } - tier4_autoware_utils::appendMarkerArray(added, &target_marker_array); + autoware_universe_utils::appendMarkerArray(added, &target_marker_array); }; debug_marker_.markers.clear(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp index 3657bab882814..ef0219b893794 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp @@ -18,10 +18,10 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include -#include #include @@ -65,7 +65,7 @@ PathWithLaneId getBackwardPath( const double lateral_distance_to_shoulder_center = current_pose_arc_coords.distance; for (size_t i = 0; i < backward_path.points.size(); ++i) { auto & p = backward_path.points.at(i).point.pose; - p = tier4_autoware_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); + p = autoware_universe_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 3acfb7a2562a8..0000790a3adde 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ -#include "tier4_autoware_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include #include @@ -27,7 +27,7 @@ namespace autoware::behavior_path_planner { using autoware_perception_msgs::msg::ObjectClassification; -using tier4_autoware_utils::getOrDeclareParameter; +using autoware_universe_utils::getOrDeclareParameter; AvoidanceParameters getParameter(rclcpp::Node * node) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 25e4eecd92440..5b391eb83d0a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include #include #include @@ -53,22 +53,22 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_rtc_msgs::msg::State; // tier4 utils functions -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::getPoint; -using tier4_autoware_utils::getPose; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcLateralDeviation; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::calcYawDeviation; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::createQuaternionFromRPY; +using autoware_universe_utils::getPoint; +using autoware_universe_utils::getPose; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using autoware_universe_utils::pose2transform; +using autoware_universe_utils::toHexString; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index a6da4d5550562..4284717f181ea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -27,6 +27,7 @@ autoware_perception_msgs autoware_route_handler autoware_rtc_interface + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension @@ -40,7 +41,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 695aa5e7d73bf..6b28e33759b6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -691,7 +691,7 @@ std::string toStrInfo(const autoware::behavior_path_planner::AvoidLineArray & ap } std::string toStrInfo(const autoware::behavior_path_planner::AvoidLine & ap) { - using tier4_autoware_utils::toHexString; + using autoware_universe_utils::toHexString; std::stringstream pids; for (const auto pid : ap.parent_ids) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index db125f066ab03..0ca2b9c4e0d21 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -29,7 +29,7 @@ namespace autoware::behavior_path_planner void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { using autoware_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::getOrDeclareParameter; + using autoware_universe_utils::getOrDeclareParameter; // init manager interface initInterface(node, {"left", "right"}); @@ -43,7 +43,7 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( const std::vector & parameters) { using autoware_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto p = parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index e54f6b0658e41..1cd3db974b831 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -755,7 +755,7 @@ bool StaticObstacleAvoidanceModule::isSafePath( auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = - tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + autoware_universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index b8e30a91d36a1..634ee0e71d737 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -365,7 +365,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; const bool has_object_near_goal = - tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) < + autoware_universe_utils::calcDistance2d(goal_pose.position, object_pos) < parameters_->object_check_goal_distance; return has_object_near_goal; }(); @@ -1026,7 +1026,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const auto has_object_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return tier4_autoware_utils::calcDistance2d( + return autoware_universe_utils::calcDistance2d( data_->route_handler->getGoalPose().position, o.object.kinematics.initial_pose_with_covariance.pose.position) < parameters_->object_check_goal_distance; @@ -1097,7 +1097,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const bool has_last_shift_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return tier4_autoware_utils::calcDistance2d( + return autoware_universe_utils::calcDistance2d( last_sl.end.position, o.object.kinematics.initial_pose_with_covariance.pose.position) < parameters_->object_check_goal_distance; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 272b97f95a746..71935f502dd03 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -55,7 +55,8 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const return p; } -geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygon, const double z) +geometry_msgs::msg::Polygon toMsg( + const autoware_universe_utils::Polygon2d & polygon, const double z) { geometry_msgs::msg::Polygon ret; for (const auto & p : polygon.outer()) { @@ -74,7 +75,7 @@ size_t findFirstNearestIndex(const T & points, const geometry_msgs::msg::Point & bool decreasing = false; for (size_t i = 0; i < points.size(); ++i) { - const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = autoware_universe_utils::calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { decreasing = true; min_dist = dist; @@ -328,7 +329,7 @@ bool isWithinIntersection( return false; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object.object); const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); @@ -645,7 +646,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object.object); const auto is_disjoint_right_lane = boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon()); if (is_disjoint_right_lane) { @@ -668,7 +669,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object.object); const auto is_disjoint_left_lane = boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon()); if (is_disjoint_left_lane) { @@ -940,8 +941,8 @@ double getRoadShoulderDistance( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data) { + using autoware_universe_utils::Point2d; using lanelet::utils::to2D; - using tier4_autoware_utils::Point2d; const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = @@ -968,7 +969,7 @@ double getRoadShoulderDistance( const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; const auto opt_intersect = - tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware_universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( @@ -986,7 +987,7 @@ double getRoadShoulderDistance( calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) .position; const auto opt_intersect = - tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware_universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( @@ -1035,8 +1036,8 @@ bool isWithinLanes( { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto transform = tier4_autoware_utils::pose2transform(ego_pose); - const auto footprint = tier4_autoware_utils::transformVector( + const auto transform = autoware_universe_utils::pose2transform(ego_pose); + const auto footprint = autoware_universe_utils::transformVector( planner_data->parameters.vehicle_info.createFootprint(), transform); lanelet::ConstLanelet closest_lanelet{}; @@ -1192,7 +1193,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( double min_distance = std::numeric_limits::max(); double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); @@ -1209,7 +1210,7 @@ std::vector> calcEnvelopeOverhangDistance( std::vector> overhang_points{}; for (const auto & p : object_data.envelope_poly.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. const auto idx = motion_utils::findNearestIndex(path.points, point); const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); @@ -1245,9 +1246,9 @@ Polygon2d createEnvelopePolygon( const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer) { namespace bg = boost::geometry; - using tier4_autoware_utils::expandPolygon; - using tier4_autoware_utils::Point2d; - using tier4_autoware_utils::Polygon2d; + using autoware_universe_utils::expandPolygon; + using autoware_universe_utils::Point2d; + using autoware_universe_utils::Polygon2d; using Box = bg::model::box; const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) { @@ -1291,7 +1292,7 @@ Polygon2d createEnvelopePolygon( Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object_data.object); return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer); } @@ -1323,7 +1324,7 @@ std::vector generateObstaclePolygonsForDrivableArea( const double diff_poly_buffer = object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = - tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); + autoware_universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); obstacles_for_drivable_area.push_back( {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); } @@ -1453,7 +1454,7 @@ void fillObjectEnvelopePolygon( const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object_data.object); const auto object_polygon_area = boost::geometry::area(object_polygon); const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly); @@ -1864,7 +1865,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( { // TODO(Horibe) parametrize const auto isSimilar = [](const AvoidLine & a, const AvoidLine & b) { - using tier4_autoware_utils::calcDistance2d; + using autoware_universe_utils::calcDistance2d; if (calcDistance2d(a.start, b.start) > 1.0) { return false; } @@ -2151,7 +2152,7 @@ std::pair separateObjectsByPath( const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto obj_polygon = autoware_universe_utils::toPolygon2d(object); if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index 8a740727445ca..cff7d952c9016 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -20,13 +20,13 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils geometry_msgs lanelet2_extension motion_utils pluginlib rclcpp tf2 - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp index 422cb95c727b4..b881447e837ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp @@ -16,8 +16,8 @@ #include #include +#include #include -#include #include @@ -27,10 +27,10 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; namespace { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 73c38562acaad..f651b25994da7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -256,7 +256,7 @@ std::optional BlindSpotModule::getSiblingStraightLanelet( static std::optional getFirstPointIntersectsLineByFootprint( const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -266,8 +266,8 @@ static std::optional getFirstPointIntersectsLineByFootprint( const auto line2d = line.basicLineString(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - footprint, tier4_autoware_utils::pose2transform(base_pose)); + const auto path_footprint = autoware_universe_utils::transformVector( + footprint, autoware_universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, line2d)) { return std::make_optional(i); } @@ -282,7 +282,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (tier4_autoware_utils::calcDistance2d(p, point) < min_dist) { + if (autoware_universe_utils::calcDistance2d(p, point) < min_dist) { return i; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 5e2a1fdf50544..420ff10b6fb5e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -26,6 +26,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils eigen geometry_msgs @@ -41,7 +42,6 @@ rclcpp sensor_msgs tier4_api_msgs - tier4_autoware_utils tier4_debug_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp index 77c00b5b64c6f..8b3d2029a4cb6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp @@ -15,23 +15,23 @@ #include "scene_crosswalk.hpp" #include +#include +#include #include -#include -#include #include namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using motion_utils::createSlowDownVirtualWallMarker; using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; namespace diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index dfd060decec8f..398368ccd51e7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -15,7 +15,7 @@ #include "manager.hpp" #include -#include +#include #include #include @@ -27,8 +27,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::autoware::Crosswalk; -using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 9ee7c7e58178c..444dd68767919 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -118,7 +118,7 @@ void clear_occlusions_behind_objects( object.kinematics.initial_pose_with_covariance.pose.position.y}; if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto object_polygon = autoware_universe_utils::toPolygon2d(object); for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); std::sort(edge_points.begin(), edge_points.end(), angle_cmp); // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7a007d023dcc9..2ee7ff186fcba 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -18,13 +18,13 @@ #include #include +#include +#include +#include #include #include #include #include -#include -#include -#include #include #include @@ -41,6 +41,12 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::getPose; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using autoware_universe_utils::pose2transform; +using autoware_universe_utils::toHexString; using motion_utils::calcArcLength; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLateralOffset; @@ -51,12 +57,6 @@ using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::getPose; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; namespace { @@ -85,7 +85,7 @@ void offsetPolygon2d( { for (const auto & polygon_point : polygon.points) { const auto offset_pos = - tier4_autoware_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + autoware_universe_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) .position; offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } @@ -99,7 +99,7 @@ Polygon2d createMultiStepPolygon( Polygon2d multi_step_polygon{}; for (size_t i = start_idx; i <= end_idx; ++i) { offsetPolygon2d( - tier4_autoware_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); + autoware_universe_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); } Polygon2d hull_multi_step_polygon{}; @@ -414,7 +414,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( if ( isVehicleType(object.classification) && ego_crosswalk_passage_direction && collision_point.crosswalk_passage_direction) { - const double direction_diff = tier4_autoware_utils::normalizeRadian( + const double direction_diff = autoware_universe_utils::normalizeRadian( collision_point.crosswalk_passage_direction.value() - ego_crosswalk_passage_direction.value()); if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { @@ -648,13 +648,14 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional> { const auto line_start = - tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware_universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + autoware_universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); for (unsigned i = 0; i < path.points.size() - 1; ++i) { const auto & start = path.points.at(i).point.pose.position; const auto & end = path.points.at(i + 1).point.pose.position; - if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + if (const auto intersect = + autoware_universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return std::make_optional(std::make_pair(i, intersect.value())); } @@ -676,18 +677,19 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( const autoware_perception_msgs::msg::PredictedPath & path) const { - using tier4_autoware_utils::Segment2d; + using autoware_universe_utils::Segment2d; auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional> { const auto line_start = - tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware_universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + autoware_universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); for (unsigned i = 0; i < path.path.size() - 1; ++i) { const auto & start = path.path.at(i).position; const auto & end = path.path.at(i + 1).position; - if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + if (const auto intersect = + autoware_universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return std::make_optional(std::make_pair(i, intersect.value())); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b67e385d2d7cf..b57c2bc4d66a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -18,10 +18,10 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" #include +#include +#include #include #include -#include -#include #include #include @@ -53,10 +53,10 @@ using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_universe_utils::Polygon2d; +using autoware_universe_utils::StopWatch; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; namespace @@ -263,8 +263,8 @@ class CrosswalkModule : public SceneModuleInterface const bool is_object_away_from_path = !attention_area.outer().empty() && - boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) > - 0.5; + boost::geometry::distance( + autoware_universe_utils::fromMsg(position).to_2d(), attention_area) > 0.5; // add new object if (objects.count(uuid) == 0) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index 033e724f3523a..08f741103c8ae 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -15,11 +15,11 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" #include +#include +#include #include #include #include -#include -#include #include @@ -48,10 +48,10 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::Line2d; +using autoware_universe_utils::Point2d; using motion_utils::calcSignedArcLength; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::Line2d; -using tier4_autoware_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, @@ -152,7 +152,7 @@ std::vector getPolygonIntersects( std::sort(intersects.begin(), intersects.end(), compare); - // convert tier4_autoware_utils::Point2d to geometry::msg::Point + // convert autoware_universe_utils::Point2d to geometry::msg::Point std::vector geometry_points; for (const auto & p : intersects) { geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); @@ -201,7 +201,7 @@ std::vector getLinestringIntersects( std::sort(intersects.begin(), intersects.end(), compare); - // convert tier4_autoware_utils::Point2d to geometry::msg::Point + // convert autoware_universe_utils::Point2d to geometry::msg::Point std::vector geometry_points; for (const auto & p : intersects) { geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index c7ec9496c49af..982246ffed499 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_velocity_planner_common autoware_planning_msgs autoware_route_handler + autoware_universe_utils eigen geometry_msgs lanelet2_extension @@ -30,7 +31,6 @@ tf2 tf2_eigen tf2_geometry_msgs - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp index abec113297a2f..2431c871633b7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp @@ -16,9 +16,9 @@ #include #include +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -31,11 +31,11 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; namespace { @@ -179,13 +179,13 @@ motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() wall.style = motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } wall.style = motion_utils::VirtualWallType::deadline; for (const auto & p : debug_data_.dead_line_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index f8ab9e1ef590f..6bd0d2c3b274a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "manager.hpp" #include +#include #include -#include #include @@ -30,8 +30,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::autoware::DetectionArea; -using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 3cfc3c09e4c57..827470317180e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -288,7 +288,7 @@ std::vector DetectionAreaModule::getObstaclePoints() (circle.first.y() - p.y) * (circle.first.y() - p.y); if (squared_dist <= circle.second) { if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) { - obstacle_points.push_back(tier4_autoware_utils::createPoint(p.x, p.y, p.z)); + obstacle_points.push_back(autoware_universe_utils::createPoint(p.x, p.y, p.z)); // get all obstacle point becomes high computation cost so skip if any point is found break; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 4e79139ba2fe7..c2b5ed9d3cfe8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -23,6 +23,7 @@ autoware_planning_msgs autoware_route_handler autoware_rtc_interface + autoware_universe_utils autoware_vehicle_info_utils fmt geometry_msgs @@ -35,7 +36,6 @@ pluginlib rclcpp tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index c97128304e85b..baf7aa806c3dc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -17,8 +17,8 @@ #include #include +#include #include -#include #include @@ -33,10 +33,10 @@ namespace { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, @@ -226,10 +226,10 @@ constexpr std::tuple light_blue() namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index be7e9e25cc8bc..470464a6156b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner void IntersectionLanelets::update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr) { is_prioritized_ = is_prioritized; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index a7ca5eb3b0bc1..af11033a454a7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -17,7 +17,7 @@ #include "interpolated_path_info.hpp" -#include +#include #include #include @@ -41,7 +41,7 @@ struct IntersectionLanelets */ void update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr); const lanelet::ConstLanelets & attention() const diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index b57914cfbb612..5ec002535f1a1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -16,8 +16,8 @@ #include #include +#include #include -#include #include @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; +using autoware_universe_utils::getOrDeclareParameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index 7473cefa94cac..cc4b7cb0658be 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -14,9 +14,9 @@ #include "object_manager.hpp" +#include +#include // for toPolygon2d #include -#include -#include // for toPolygon2d #include #include @@ -38,15 +38,15 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) return ss.str(); } -tier4_autoware_utils::Polygon2d createOneStepPolygon( +autoware_universe_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, const autoware_perception_msgs::msg::Shape & shape) { namespace bg = boost::geometry; - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + const auto prev_poly = autoware_universe_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = autoware_universe_utils::toPolygon2d(next_pose, shape); - tier4_autoware_utils::Polygon2d one_step_poly; + autoware_universe_utils::Polygon2d one_step_poly; for (const auto & point : prev_poly.outer()) { one_step_poly.outer().push_back(point); } @@ -56,7 +56,7 @@ tier4_autoware_utils::Polygon2d createOneStepPolygon( bg::correct(one_step_poly); - tier4_autoware_utils::Polygon2d convex_one_step_poly; + autoware_universe_utils::Polygon2d convex_one_step_poly; bg::convex_hull(one_step_poly, convex_one_step_poly); return convex_one_step_poly; @@ -164,13 +164,13 @@ bool ObjectInfo::can_stop_before_ego_lane( const auto stopline = stopline_opt.value(); const auto stopline_p1 = stopline.front(); const auto stopline_p2 = stopline.back(); - const tier4_autoware_utils::Point2d stopline_mid{ + const autoware_universe_utils::Point2d stopline_mid{ (stopline_p1.x() + stopline_p2.x()) / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; const auto attention_lane_end = attention_lanelet.centerline().back(); - const tier4_autoware_utils::LineString2d attention_lane_later_part( - {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, - tier4_autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); - std::vector ego_collision_points; + const autoware_universe_utils::LineString2d attention_lane_later_part( + {autoware_universe_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + autoware_universe_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; bg::intersection( attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); if (ego_collision_points.empty()) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 0dd51d68914ef..b7c55dffec5da 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,13 +18,13 @@ #include // for toGeomPoly #include +#include // for toPolygon2d +#include +#include #include #include #include #include -#include // for toPolygon2d -#include -#include #include @@ -56,7 +56,7 @@ IntersectionModule::IntersectionModule( associative_ids_(associative_ids), turn_direction_(turn_direction), has_traffic_light_(has_traffic_light), - occlusion_uuid_(tier4_autoware_utils::generateUUID()) + occlusion_uuid_(autoware_universe_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 7bad94bf3fdb6..6e0fc2958d4f9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -17,11 +17,11 @@ #include // for toGeomPoly #include // for smoothPath +#include // for toPolygon2d +#include #include #include #include -#include // for toPolygon2d -#include #include #include @@ -111,7 +111,7 @@ void IntersectionModule::updateObjectInfoManagerArea() return false; } return bg::within( - tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + autoware_universe_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); }(); std::optional attention_lanelet{std::nullopt}; std::optional stopline{std::nullopt}; @@ -311,7 +311,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( for (auto i = begin; i <= end; ++i) { if (bg::intersects( polygon, - tier4_autoware_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + autoware_universe_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { collision_detected = true; break; } @@ -490,13 +490,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_1st_judge_line_pose.position.x, // 4 - passed_1st_judge_line_pose.position.y, // 5 - tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + autoware_universe_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 ); } } @@ -528,13 +528,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_2nd_judge_line_pose.position.x, // 4 - passed_2nd_judge_line_pose.position.y, // 5 - tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + autoware_universe_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 ); } } @@ -615,9 +615,9 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( const auto &p1 = unsafe_interval.path.at(begin).position, p2 = unsafe_interval.path.at(end).position; const auto collision_pos = - tier4_autoware_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + autoware_universe_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); const auto object_dist_to_margin_point = - tier4_autoware_utils::calcDistance2d( + autoware_universe_utils::calcDistance2d( object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, collision_pos) - planner_param_.collision_detection.avoid_collision_by_acceleration @@ -779,7 +779,8 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + const double angle_diff = + autoware_universe_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return std::make_optional(i); @@ -916,7 +917,7 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin const auto & p1 = smoothed_reference_path.points.at(i); const auto & p2 = smoothed_reference_path.points.at(i + 1); - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + const double dist = autoware_universe_utils::calcDistance2d(p1, p2); dist_sum += dist; // use average velocity between p1 and p2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index c43f8617897b9..f05b49a52b165 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" +#include // for toPolygon2d #include -#include // for toPolygon2d #include #include @@ -151,7 +151,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( auto findCommonCvPolygons = [&](const auto & area2d, std::vector> & cv_polygons) -> void { - tier4_autoware_utils::Polygon2d area2d_poly; + autoware_universe_utils::Polygon2d area2d_poly; for (const auto & p : area2d) { area2d_poly.outer().emplace_back(p.x(), p.y()); } @@ -240,7 +240,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( std::vector> blocking_polygons; for (const auto & blocking_attention_object_info : blocking_attention_objects) { const Polygon2d obj_poly = - tier4_autoware_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); + autoware_universe_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -321,7 +321,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { const auto path_linestring_point = path_ip.points.at(i).point.pose.position; if ( - tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + autoware_universe_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < 1.0 /* rough tick for computational cost */) { continue; } @@ -405,9 +405,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( division_index, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), - tier4_autoware_utils::createPoint( + autoware_universe_utils::createPoint(point_it->x(), point_it->y(), origin.z), + autoware_universe_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), + autoware_universe_utils::createPoint( projection_it->x(), projection_it->y(), origin.z) /* initialize with projection point at first*/}; found_min_dist_for_this_division = true; @@ -416,7 +416,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // division previously in this iteration, and the iterated cells are still OCCLUDED since // then nearest_occlusion_point.visible_end = - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z); + autoware_universe_utils::createPoint(point_it->x(), point_it->y(), origin.z); } } is_prev_occluded = (pixel == OCCLUDED); @@ -440,7 +440,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { const auto obj_poly = - tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); + autoware_universe_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_triangle)) { debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 8c7a4272d1d68..657276f00ae84 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -31,7 +31,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware_universe_utils { template <> @@ -44,7 +44,7 @@ inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) return point; } -} // namespace tier4_autoware_utils +} // namespace autoware_universe_utils namespace { @@ -74,7 +74,7 @@ lanelet::ConstLanelet generatePathLanelet( for (size_t i = start_idx; i <= end_idx; ++i) { const auto & p = path.points.at(i).point.pose; const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + if (i != start_idx && autoware_universe_utils::calcDistance2d(p_prev, p) < interval) { continue; } prev_idx = i; @@ -277,7 +277,7 @@ Result IntersectionModule::prepare const bool approached_assigned_lane = motion_utils::calcSignedArcLength( path->points, closest_idx, - tier4_autoware_utils::createPoint( + autoware_universe_utils::createPoint( assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), assigned_lane_begin_point.z())) < planner_param_.collision_detection.yield_on_green_traffic_light @@ -381,8 +381,8 @@ std::optional IntersectionModule::generateIntersectionSto std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + const auto path_footprint = autoware_universe_utils::transformVector( + local_footprint, autoware_universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { // NOTE: maybe consideration of braking dist is necessary first_footprint_attention_centerline_ip_opt = i; @@ -423,8 +423,8 @@ std::optional IntersectionModule::generateIntersectionSto // NOTE: if footprints[0] is already inside the attention area, invalid { const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + const auto path_footprint0 = autoware_universe_utils::transformVector( + local_footprint, autoware_universe_utils::pose2transform(base_pose0)); if (bg::intersects( path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { occlusion_peeking_line_valid = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 7d6dd09bb8b6e..4d13890be5c64 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -290,7 +290,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & pa } // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const auto obj_footprint = autoware_universe_utils::toPolygon2d(object); // NOTE: in order not to stop too much const bool is_in_stuck_area = bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 87d4065281ffa..f675d24e5e162 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -53,7 +53,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); @@ -63,8 +63,8 @@ static std::optional getFirstConflictingLanelet( for (size_t i = start; i <= end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + const auto path_footprint = autoware_universe_utils::transformVector( + footprint, autoware_universe_utils::pose2transform(pose)); for (const auto & conflicting_lanelet : conflicting_lanelets) { const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); const bool intersects = bg::intersects(polygon_2d, path_footprint); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index 66c82b5c4a1e5..85751442589f4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -51,7 +51,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (tier4_autoware_utils::calcDistance2d(p, point) < min_dist) { + if (autoware_universe_utils::calcDistance2d(p, point) < min_dist) { return i; } } @@ -130,7 +130,7 @@ std::optional> findLaneIdsInterval( std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -140,8 +140,8 @@ std::optional getFirstPointInsidePolygonByFootprint( const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - footprint, tier4_autoware_utils::pose2transform(base_pose)); + const auto path_footprint = autoware_universe_utils::transformVector( + footprint, autoware_universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, area_2d)) { return std::make_optional(i); } @@ -154,7 +154,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -164,8 +164,8 @@ getFirstPointInsidePolygonsByFootprint( for (size_t i = start; i <= lane_end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + const auto path_footprint = autoware_universe_utils::transformVector( + footprint, autoware_universe_utils::pose2transform(pose)); for (size_t j = 0; j < polygons.size(); ++j) { const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); const bool is_in_polygon = bg::intersects(area_2d, path_footprint); @@ -332,7 +332,7 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp index ef826380afa69..1907395b4baee 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -17,8 +17,8 @@ #include "interpolated_path_info.hpp" +#include #include -#include #include @@ -87,7 +87,7 @@ bool isBeforeTargetIndex( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** @@ -125,7 +125,7 @@ mergeLaneletsByTopologicalSort( */ std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length); /** * @brief find the index of the first point where vehicle footprint intersects with the given @@ -136,7 +136,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware_universe_utils::LinearRing2d & footprint, const double vehicle_length); std::vector getPolygon3dFromLanelets( const lanelet::ConstLanelets & ll_vec); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 1a2d435fb6a4a..df64b5d8a4f6b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -21,6 +21,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils eigen geometry_msgs @@ -33,7 +34,6 @@ tf2 tf2_eigen tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp index 55a13f282662e..488132113d2d3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -16,9 +16,9 @@ #include #include +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -34,10 +34,10 @@ namespace { const double marker_lifetime = 0.2; using DebugData = NoStoppingAreaModule::DebugData; -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; lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) { @@ -170,7 +170,7 @@ motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() wall.text = "no_stopping_area"; wall.style = motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 0c7d762693d17..773ec5f44411e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "manager.hpp" #include +#include #include -#include #include @@ -30,8 +30,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::autoware::NoStoppingArea; -using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 648d8a7800cb3..7ece8dd8ace9c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -17,10 +17,10 @@ #include #include #include +#include +#include #include #include -#include -#include #include #include @@ -87,7 +87,7 @@ boost::optional NoStoppingAreaModule::getStopLineGeometry2d( std::vector collision_points; bg::intersection(area_poly, line, collision_points); if (!collision_points.empty()) { - const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1); + const double yaw = autoware_universe_utils::calcAzimuthAngle(p0, p1); const double w = planner_data_->vehicle_info_.vehicle_width_m; const double l = stop_line_margin; stop_line.emplace_back( @@ -229,7 +229,7 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( continue; // not stop vehicle } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const Polygon2d obj_footprint = autoware_universe_utils::toPolygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); @@ -318,7 +318,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } const auto no_stopping_area = no_stopping_area_reg_elem_.noStoppingAreas().front(); for (size_t i = closest_idx + num_ignore_nearest; i < pp.size() - 1; ++i) { - dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware_universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { is_in_area = true; @@ -339,10 +339,10 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( // decide end idx with extract distance size_t ego_area_end_idx = ego_area_start_idx; for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware_universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - dist_from_area_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_area_sum += autoware_universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); } if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { break; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index eaffade354061..116fb72176a30 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -21,6 +21,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs grid_map_ros @@ -34,7 +35,6 @@ rclcpp tf2 tf2_geometry_msgs - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp index 983bf3fc1fe10..6a05178ee769c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -17,8 +17,8 @@ #include #include +#include #include -#include #include #include @@ -30,13 +30,13 @@ namespace { using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerPosition; +using autoware_universe_utils::createMarkerScale; using occlusion_spot_utils::PossibleCollisionInfo; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerPosition; -using tier4_autoware_utils::createMarkerScale; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 0557922da53fe..03124a0b81b43 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -95,7 +95,7 @@ std::optional generateOcclusionPolygon( const Polygon2d & occupancy_poly, const Point2d & origin, const Point2d & min_theta_pos, const Point2d & max_theta_pos, const double ray_max_length = 100.0) { - using tier4_autoware_utils::normalizeRadian; + using autoware_universe_utils::normalizeRadian; const double origin_x = origin.x(); const double origin_y = origin.y(); const double min_theta = @@ -134,7 +134,7 @@ std::optional generateOcclusionPolygon( Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, const double r = 100) { - using tier4_autoware_utils::calcOffsetPose; + using autoware_universe_utils::calcOffsetPose; // generate occupancy polygon from grid origin Polygon2d poly; // create counter clockwise poly poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, 0, 0).position)); @@ -155,7 +155,7 @@ std::pair calcEdgePoint(const Polygon2d & foot_print, const Poin for (size_t i = 0; i < foot_print.outer().size(); i++) { const auto & f = foot_print.outer().at(i); PolarCoordinates polar = toPolarCoordinates(origin, f); - const double theta_norm = tier4_autoware_utils::normalizeRadian(polar.theta, 0.0); + const double theta_norm = autoware_universe_utils::normalizeRadian(polar.theta, 0.0); if (theta_norm < min_theta) { min_theta = theta_norm; min_idx = i; @@ -183,7 +183,7 @@ std::optional generateOccupiedPolygon( Point transformFromMap2Grid(const TransformStamped & geom_tf_map2grid, const Point2d & p) { - Point geom_pt = tier4_autoware_utils::createPoint(p.x(), p.y(), 0); + Point geom_pt = autoware_universe_utils::createPoint(p.x(), p.y(), 0); Point transformed_geom_pt; // from map coordinate to occupancy grid coordinate tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index b991444c949fc..60ef9c629d830 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -17,13 +17,13 @@ #include #include +#include +#include #include #include #include #include #include -#include -#include #include #include @@ -57,13 +57,13 @@ namespace grid_utils { using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; using nav_msgs::msg::OccupancyGrid; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; namespace occlusion_cost_value { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index f13fb09b751fd..f21d2bf6a743b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -17,7 +17,7 @@ #include "scene_occlusion_spot.hpp" #include -#include +#include #include @@ -27,9 +27,9 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; -using tier4_autoware_utils::getOrDeclareParameter; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 0b06702c41726..cade9f0614dc0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -18,11 +18,11 @@ #include #include +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -41,7 +41,7 @@ namespace occlusion_spot_utils Polygon2d toFootprintPolygon(const PredictedObject & object, const double scale = 1.0) { const Pose & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object); + Polygon2d obj_footprint = autoware_universe_utils::toPolygon2d(object); // upscale foot print for noise obj_footprint = upScalePolygon(obj_pose.position, obj_footprint, scale); return obj_footprint; @@ -126,7 +126,7 @@ void calcSlowDownPointsForPossibleCollision( const double dist_to_col = possible_collisions.at(collision_index).arc_lane_dist_at_collision.length; dist_along_next_path_point += - tier4_autoware_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); + autoware_universe_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); // process if nearest possible collision is between current and next path point if (dist_along_path_point < dist_to_col) { for (; collision_index < possible_collisions.size(); collision_index++) { @@ -173,7 +173,7 @@ void clipPathByLength( double length_sum = 0; clipped.points.emplace_back(path.points.front()); for (int i = 1; i < static_cast(path.points.size()); i++) { - length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); + length_sum += autoware_universe_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); if (length_sum > max_length) return; clipped.points.emplace_back(path.points.at(i)); } @@ -248,7 +248,7 @@ void categorizeVehicles( lanelet::ArcCoordinates getOcclusionPoint( const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string) { - const auto poly = tier4_autoware_utils::toPolygon2d(obj); + const auto poly = autoware_universe_utils::toPolygon2d(obj); std::deque arcs; for (const auto & p : poly.outer()) { lanelet::BasicPoint2d obj_p = {p.x(), p.y()}; @@ -374,7 +374,7 @@ std::vector filterVehiclesByDetectionArea( // stuck points by predicted objects for (const auto & object : objs) { // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const auto obj_footprint = autoware_universe_utils::toPolygon2d(object); for (const auto & p : polys) { if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); @@ -402,8 +402,8 @@ bool generatePossibleCollisionsFromGridMap( param.detection_area.min_occlusion_spot_size); if (param.is_show_occlusion) { for (const auto & op : occlusion_spot_positions) { - Point p = - tier4_autoware_utils::createPoint(op[0], op[1], path.points.at(0).point.pose.position.z); + Point p = autoware_universe_utils::createPoint( + op[0], op[1], path.points.at(0).point.pose.position.z); debug_data.occlusion_points.emplace_back(p); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 8e7060ba2932c..d6f39dd56bdee 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -19,8 +19,8 @@ #include #include +#include #include -#include #include #include @@ -61,7 +61,7 @@ class OcclusionSpotModule : public SceneModuleInterface private: // Parameter PlannerParam param_; - tier4_autoware_utils::StopWatch stop_watch_; + autoware_universe_utils::StopWatch stop_watch_; std::vector partition_lanelets_; protected: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index b19505cc4b82f..b0f4a48b9dbdc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -16,7 +16,7 @@ #include "utils.hpp" #include -#include +#include #include @@ -85,7 +85,7 @@ TEST(compareTime, polygon_vs_line_iterator) } } const grid_map::Matrix & grid_data = grid["layer"]; - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; stop_watch.tic("processing_time"); size_t count = 0; [[maybe_unused]] double time = 0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 376ac6cf5bf31..271efdc1a1b42 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -39,6 +39,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_velocity_smoother diagnostic_msgs eigen @@ -56,7 +57,6 @@ tf2_geometry_msgs tf2_ros tier4_api_msgs - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index ba41f7afdf15f..9ed221f9df0bc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -15,12 +15,12 @@ #include "node.hpp" #include +#include +#include #include #include #include #include -#include -#include #include #include @@ -130,8 +130,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchScenePlugin(*this, name); } - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -193,7 +194,7 @@ void BehaviorVelocityPlannerNode::processNoGroundPointCloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); if (!pc.empty()) { - tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + autoware_universe_utils::transformPointCloud(pc, *pc_transformed, affine); } planner_data_.no_ground_pointcloud = pc_transformed; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp index 20cb452c86cd8..1db2766fdf686 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp @@ -15,15 +15,15 @@ #ifndef NODE_HPP_ #define NODE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "planner_manager.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include +#include #include #include #include -#include #include #include @@ -69,30 +69,30 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; // polling subscribers - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::PredictedObjects> sub_predicted_objects_{this, "~/input/dynamic_objects"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ - this, "~/input/no_ground_pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()}; + this, "~/input/no_ground_pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_acceleration_{this, "~/input/accel"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); @@ -143,9 +143,9 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; static constexpr int logger_throttle_interval = 3000; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index bdc37e794ca8a..f9a6210d74019 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -18,11 +18,11 @@ #include #include #include +#include +#include #include #include #include -#include -#include #include #include @@ -51,11 +51,11 @@ namespace autoware::behavior_velocity_planner using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; +using autoware_universe_utils::DebugPublisher; +using autoware_universe_utils::getOrDeclareParameter; using builtin_interfaces::msg::Time; using motion_utils::PlanningBehavior; using motion_utils::VelocityFactor; -using tier4_autoware_utils::DebugPublisher; -using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 8e5f88973ab13..09a9125ef0eac 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ #include -#include +#include #include @@ -31,7 +31,7 @@ namespace autoware::behavior_velocity_planner { namespace { -geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point2d & p) +geometry_msgs::msg::Point convertToGeomPoint(const autoware_universe_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -46,7 +46,7 @@ namespace arc_lane_utils { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = - std::pair; // front index, point2d + std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset @@ -76,9 +76,9 @@ std::optional findCollisionSegment( } const auto & p1 = - tier4_autoware_utils::getPoint(path.points.at(i)); // Point before collision point + autoware_universe_utils::getPoint(path.points.at(i)); // Point before collision point const auto & p2 = - tier4_autoware_utils::getPoint(path.points.at(i + 1)); // Point after collision point + autoware_universe_utils::getPoint(path.points.at(i + 1)); // Point after collision point const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); @@ -106,7 +106,7 @@ std::optional findForwardOffsetSegment( { double sum_length = 0.0; for (size_t i = base_idx; i < path.points.size() - 1; ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += autoware_universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -125,7 +125,7 @@ std::optional findBackwardOffsetSegment( double sum_length = 0.0; const auto start = static_cast(base_idx) - 1; for (std::int32_t i = start; i >= 0; --i) { - sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += autoware_universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -149,13 +149,13 @@ std::optional findOffsetSegment( return findForwardOffsetSegment( path, collision_idx, offset_length + - tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); + autoware_universe_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); } return findBackwardOffsetSegment( path, collision_idx + 1, -offset_length + - tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); + autoware_universe_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } std::optional findOffsetSegment( @@ -185,8 +185,8 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse target_pose.position.x = target_point_2d.x(); target_pose.position.y = target_point_2d.y(); target_pose.position.z = interpolated_z; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_back); - target_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware_universe_utils::calcAzimuthAngle(p_front, p_back); + target_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); return target_pose; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index cf72334c7b993..608d136a418bb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#include +#include #include #include @@ -50,9 +50,9 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using Point2d = tier4_autoware_utils::Point2d; -using LineString2d = tier4_autoware_utils::LineString2d; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Point2d = autoware_universe_utils::Point2d; +using LineString2d = autoware_universe_utils::LineString2d; +using Polygon2d = autoware_universe_utils::Polygon2d; template Point2d to_bg2d(const T & p) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index 78e032a918ac3..9dd3792e2799a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#include +#include #include #include @@ -57,9 +57,9 @@ struct TrafficSignalStamped }; using Pose = geometry_msgs::msg::Pose; -using Point2d = tier4_autoware_utils::Point2d; -using LineString2d = tier4_autoware_utils::LineString2d; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Point2d = autoware_universe_utils::Point2d; +using LineString2d = autoware_universe_utils::LineString2d; +using Polygon2d = autoware_universe_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; using autoware_perception_msgs::msg::PredictedObjects; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 69b153ad5d9a9..96bad9b21b27c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -26,6 +26,7 @@ autoware_planning_msgs autoware_route_handler autoware_rtc_interface + autoware_universe_utils autoware_vehicle_info_utils autoware_velocity_smoother diagnostic_msgs @@ -44,7 +45,6 @@ tf2_geometry_msgs tf2_ros tier4_api_msgs - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index 95e8978cd6136..d70d56509cc47 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -14,9 +14,9 @@ #include #include +#include +#include #include -#include -#include #include #include @@ -24,7 +24,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::StopWatch; +using autoware_universe_utils::StopWatch; SceneModuleInterface::SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) @@ -236,7 +236,7 @@ UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) cons void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) { - map_uuid_.insert({module_id, tier4_autoware_utils::generateUUID()}); + map_uuid_.insert({module_id, autoware_universe_utils::generateUUID()}); } void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index fce9b04e5a4cb..3e029174b288d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index c0c5ff12cc981..9061508e6d77d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -14,16 +14,16 @@ #include #include -#include +#include namespace autoware::behavior_velocity_planner { namespace debug { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 0043475e07954..a189dd52a8c90 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -79,7 +79,7 @@ autoware_planning_msgs::msg::Path interpolatePath( v.push_back(path_point.longitudinal_velocity_mps); if (idx != 0) { const auto path_point_prev = path.points.at(idx - 1); - s += tier4_autoware_utils::calcDistance2d(path_point_prev.pose, path_point.pose); + s += autoware_universe_utils::calcDistance2d(path_point_prev.pose, path_point.pose); } if (s > path_len) { break; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index 56b784d20b084..16a8cd1e69be8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -45,8 +45,10 @@ size_t calcPointIndexFromSegmentIndex( const size_t prev_point_idx = seg_idx; const size_t next_point_idx = seg_idx + 1; - const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx)); - const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx)); + const double prev_dist = + autoware_universe_utils::calcDistance2d(point, points.at(prev_point_idx)); + const double next_dist = + autoware_universe_utils::calcDistance2d(point, points.at(next_point_idx)); if (prev_dist < next_dist) { return prev_point_idx; @@ -60,7 +62,7 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con { auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; PathPoint p; - p.pose = tier4_autoware_utils::calcInterpolatedPose(p0, p1, ratio); + p.pose = autoware_universe_utils::calcInterpolatedPose(p0, p1, ratio); const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); p.longitudinal_velocity_mps = v; return p; @@ -82,7 +84,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); res.position.z = target.position.z - origin.position.z; res.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); + autoware_universe_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); return res; } @@ -94,15 +96,15 @@ namespace autoware::behavior_velocity_planner namespace planning_utils { using autoware_planning_msgs::msg::PathPoint; +using autoware_universe_utils::calcAzimuthAngle; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::calcSquaredDistance2d; +using autoware_universe_utils::createQuaternionFromYaw; +using autoware_universe_utils::getPoint; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; using motion_utils::validateNonEmpty; -using tier4_autoware_utils::calcAzimuthAngle; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::calcSquaredDistance2d; -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::getPoint; size_t calcSegmentIndexFromPointIndex( const std::vector & points, @@ -315,7 +317,7 @@ geometry_msgs::msg::Pose getAheadPose( for (size_t i = start_idx; i < path.points.size() - 1; ++i) { const geometry_msgs::msg::Pose p0 = path.points.at(i).point.pose; const geometry_msgs::msg::Pose p1 = path.points.at(i + 1).point.pose; - curr_dist += tier4_autoware_utils::calcDistance2d(p0, p1); + curr_dist += autoware_universe_utils::calcDistance2d(p0, p1); if (curr_dist > ahead_dist) { const double dl = std::max(curr_dist - prev_dist, 0.0001 /* avoid 0 divide */); const double w_p0 = (curr_dist - ahead_dist) / dl; @@ -635,7 +637,7 @@ std::optional insertDecelPoint( output.points.at(i).point.longitudinal_velocity_mps = std::min(original_velocity, target_velocity); } - return tier4_autoware_utils::getPose(output.points.at(insert_idx.value())); + return autoware_universe_utils::getPose(output.points.at(insert_idx.value())); } // TODO(murooka): remove this function for u-turn and crossing-path @@ -649,7 +651,7 @@ std::optional insertStopPoint( return {}; } - return tier4_autoware_utils::getPose(output.points.at(insert_idx.value())); + return autoware_universe_utils::getPose(output.points.at(insert_idx.value())); } std::optional insertStopPoint( @@ -661,7 +663,7 @@ std::optional insertStopPoint( return {}; } - return tier4_autoware_utils::getPose(output.points.at(insert_idx.value())); + return autoware_universe_utils::getPose(output.points.at(insert_idx.value())); } std::set getAssociativeIntersectionLanelets( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index 433c1470c4bbe..a1feae8c4113c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -24,6 +24,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils eigen geometry_msgs @@ -38,7 +39,6 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 129d5299385fd..9346d05128c69 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -16,17 +16,17 @@ #include "scene.hpp" +#include +#include #include -#include -#include -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; namespace autoware::behavior_velocity_planner { @@ -35,7 +35,7 @@ namespace RunOutDebug::TextWithPosition createDebugText( const std::string text, const geometry_msgs::msg::Pose pose, const float lateral_offset) { - const auto offset_pose = tier4_autoware_utils::calcOffsetPose(pose, 0, lateral_offset, 0); + const auto offset_pose = autoware_universe_utils::calcOffsetPose(pose, 0, lateral_offset, 0); RunOutDebug::TextWithPosition text_with_position; text_with_position.text = text; @@ -136,7 +136,7 @@ void RunOutDebug::pushDetectionAreaPolygons(const Polygon2d & debug_polygon) { std::vector ros_points; for (const auto & p : debug_polygon.outer()) { - ros_points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + ros_points.push_back(autoware_universe_utils::createPoint(p.x(), p.y(), 0.0)); } detection_area_polygons_.push_back(ros_points); @@ -146,7 +146,7 @@ void RunOutDebug::pushMandatoryDetectionAreaPolygons(const Polygon2d & debug_pol { std::vector ros_points; for (const auto & p : debug_polygon.outer()) { - ros_points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + ros_points.push_back(autoware_universe_utils::createPoint(p.x(), p.y(), 0.0)); } mandatory_detection_area_polygons_.push_back(ros_points); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 5e8b0382a07c0..fe4162504095f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -14,12 +14,12 @@ #include "dynamic_obstacle.hpp" +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include #include #include @@ -42,12 +42,12 @@ geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory( const auto & nearest_pose = path_points.at(nearest_idx).point.pose; const auto longitudinal_offset = - tier4_autoware_utils::calcLongitudinalDeviation(nearest_pose, point); + autoware_universe_utils::calcLongitudinalDeviation(nearest_pose, point); const auto vertical_point = - tier4_autoware_utils::calcOffsetPose(nearest_pose, longitudinal_offset, 0, 0).position; - const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle(point, vertical_point); + autoware_universe_utils::calcOffsetPose(nearest_pose, longitudinal_offset, 0, 0).position; + const auto azimuth_angle = autoware_universe_utils::calcAzimuthAngle(point, vertical_point); - return tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); + return autoware_universe_utils::createQuaternionFromYaw(azimuth_angle); } // create predicted path assuming that obstacles move with constant velocity @@ -60,7 +60,7 @@ std::vector createPredictedPath( for (size_t i = 0; i < path_size; i++) { const float travel_dist = max_velocity_mps * time_step * i; const auto predicted_pose = - tier4_autoware_utils::calcOffsetPose(initial_pose, travel_dist, 0, 0); + autoware_universe_utils::calcOffsetPose(initial_pose, travel_dist, 0, 0); path_points.emplace_back(predicted_pose); } @@ -115,7 +115,7 @@ bool isAheadOf( const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose & base_pose) { const auto longitudinal_deviation = - tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point); + autoware_universe_utils::calcLongitudinalDeviation(base_pose, target_point); const bool is_ahead = longitudinal_deviation > 0; return is_ahead; } @@ -136,7 +136,7 @@ pcl::PointCloud extractObstaclePointsWithinPolygon( pcl::PointCloud output_points; output_points.header = input_points.header; for (const auto & poly : polys) { - const auto bounding_box = bg::return_envelope(poly); + const auto bounding_box = bg::return_envelope(poly); for (const auto & p : input_points) { Point2d point(p.x, p.y); @@ -165,7 +165,7 @@ std::vector> groupPointsWithNearestSegmentIndex( points_with_index.resize(path_points.size()); for (const auto & p : input_points.points) { - const auto ros_point = tier4_autoware_utils::createPoint(p.x, p.y, p.z); + const auto ros_point = autoware_universe_utils::createPoint(p.x, p.y, p.z); const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(path_points, ros_point); // if the point is ahead of end of the path, index should be path.size() - 1 @@ -188,10 +188,10 @@ pcl::PointXYZ calculateLateralNearestPoint( { const auto lateral_nearest_point = std::min_element( input_points.points.begin(), input_points.points.end(), [&](const auto & p1, const auto & p2) { - const auto lateral_deviation_p1 = std::abs(tier4_autoware_utils::calcLateralDeviation( - base_pose, tier4_autoware_utils::createPoint(p1.x, p1.y, 0))); - const auto lateral_deviation_p2 = std::abs(tier4_autoware_utils::calcLateralDeviation( - base_pose, tier4_autoware_utils::createPoint(p2.x, p2.y, 0))); + const auto lateral_deviation_p1 = std::abs(autoware_universe_utils::calcLateralDeviation( + base_pose, autoware_universe_utils::createPoint(p1.x, p1.y, 0))); + const auto lateral_deviation_p2 = std::abs(autoware_universe_utils::calcLateralDeviation( + base_pose, autoware_universe_utils::createPoint(p2.x, p2.y, 0))); return lateral_deviation_p1 < lateral_deviation_p2; }); @@ -266,7 +266,7 @@ pcl::PointCloud transformPointCloud( pcl::PointCloud pointcloud_pcl; pcl::fromROSMsg(input_pointcloud, pointcloud_pcl); pcl::PointCloud pointcloud_pcl_transformed; - tier4_autoware_utils::transformPointCloud( + autoware_universe_utils::transformPointCloud( pointcloud_pcl, pointcloud_pcl_transformed, transform_matrix); return pointcloud_pcl_transformed; @@ -365,8 +365,8 @@ std::vector DynamicObstacleCreatorForObject::createDynamicObsta dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose; if (param_.assume_fixed_velocity) { - dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware_universe_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware_universe_utils::kmph2mps(param_.max_vel_kmph); } else { calculateMinAndMaxVelFromCovariance( predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier, @@ -408,8 +408,8 @@ std::vector DynamicObstacleCreatorForObjectWithoutPath::createD dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); - dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware_universe_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware_universe_utils::kmph2mps(param_.max_vel_kmph); dynamic_obstacle.classifications = predicted_object.classification; dynamic_obstacle.shape = predicted_object.shape; @@ -467,12 +467,13 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta DynamicObstacle dynamic_obstacle; // create pose facing the direction of the lane - dynamic_obstacle.pose.position = tier4_autoware_utils::createPoint(point.x, point.y, point.z); + dynamic_obstacle.pose.position = + autoware_universe_utils::createPoint(point.x, point.y, point.z); dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); - dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware_universe_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware_universe_utils::kmph2mps(param_.max_vel_kmph); // create classification of points as pedestrian ObjectClassification classification; @@ -493,7 +494,7 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta param_.max_prediction_time); predicted_path.confidence = 1.0; dynamic_obstacle.predicted_paths.emplace_back(predicted_path); - dynamic_obstacle.uuid = tier4_autoware_utils::generateUUID(); + dynamic_obstacle.uuid = autoware_universe_utils::generateUUID(); dynamic_obstacles.emplace_back(dynamic_obstacle); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4da58522165d6..00a44a0cf625e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -22,7 +22,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; +using autoware_universe_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index 50e60d71f7249..7c3f538c4253b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -19,10 +19,10 @@ #include #include +#include +#include #include #include -#include -#include #include #include @@ -177,7 +177,7 @@ bool RunOutModule::modifyPathVelocity( insertStopPoint(last_stop_point_, *path); // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( *last_stop_point_, planner_param_.vehicle_param.base_to_front, 0, 0)); } } @@ -222,7 +222,7 @@ std::optional RunOutModule::detectCollision( const auto & p2 = path.points.at(idx).point; const float prev_vel = std::max(p1.longitudinal_velocity_mps, planner_param_.run_out.min_vel_ego_kmph / 3.6f); - const float ds = tier4_autoware_utils::calcDistance2d(p1, p2); + const float ds = autoware_universe_utils::calcDistance2d(p1, p2); // calculate travel time from nearest point to p2 travel_time += ds / prev_vel; @@ -316,10 +316,10 @@ std::optional RunOutModule::findNearestCollisionObstacle( float RunOutModule::calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const { - const auto vehicle_front_pose = tier4_autoware_utils::calcOffsetPose( + const auto vehicle_front_pose = autoware_universe_utils::calcOffsetPose( base_pose, planner_param_.vehicle_param.base_to_front, 0, 0); const auto longitudinal_offset_from_front = - std::abs(tier4_autoware_utils::calcLongitudinalDeviation(vehicle_front_pose, point)); + std::abs(autoware_universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point)); return longitudinal_offset_from_front; } @@ -341,7 +341,7 @@ std::vector RunOutModule::createVehiclePolygon( const double base_to_left = (planner_param_.vehicle_param.wheel_tread / 2.0) + planner_param_.vehicle_param.left_overhang; - using tier4_autoware_utils::calcOffsetPose; + using autoware_universe_utils::calcOffsetPose; const auto p1 = calcOffsetPose(base_pose, base_to_front, base_to_left, 0.0); const auto p2 = calcOffsetPose(base_pose, base_to_front, -base_to_right, 0.0); const auto p3 = calcOffsetPose(base_pose, -base_to_rear, -base_to_right, 0.0); @@ -359,8 +359,8 @@ std::vector RunOutModule::createVehiclePolygon( std::vector RunOutModule::excludeObstaclesOnEgoPath( const std::vector & dynamic_obstacles, const PathWithLaneId & path) { + using autoware_universe_utils::calcOffsetPose; using motion_utils::findNearestIndex; - using tier4_autoware_utils::calcOffsetPose; if (!planner_param_.run_out.exclude_obstacles_already_in_path || path.points.empty()) { return dynamic_obstacles; } @@ -379,18 +379,18 @@ std::vector RunOutModule::excludeObstaclesOnEgoPath( } const auto object_position = o.pose.position; const auto closest_ego_pose = path.points.at(*idx).point.pose; - const auto vehicle_left_pose = - tier4_autoware_utils::calcOffsetPose(closest_ego_pose, 0, vehicle_with_with_margin_halved, 0); - const auto vehicle_right_pose = tier4_autoware_utils::calcOffsetPose( + const auto vehicle_left_pose = autoware_universe_utils::calcOffsetPose( + closest_ego_pose, 0, vehicle_with_with_margin_halved, 0); + const auto vehicle_right_pose = autoware_universe_utils::calcOffsetPose( closest_ego_pose, 0, -vehicle_with_with_margin_halved, 0); const double signed_distance_from_left = - tier4_autoware_utils::calcLateralDeviation(vehicle_left_pose, object_position); + autoware_universe_utils::calcLateralDeviation(vehicle_left_pose, object_position); const double signed_distance_from_right = - tier4_autoware_utils::calcLateralDeviation(vehicle_right_pose, object_position); + autoware_universe_utils::calcLateralDeviation(vehicle_right_pose, object_position); // If object is outside of the ego path, include it in list of possible target objects // It is also deleted from the path of objects inside ego path - const auto obstacle_uuid_hex = tier4_autoware_utils::toHexString(o.uuid); + const auto obstacle_uuid_hex = autoware_universe_utils::toHexString(o.uuid); if (signed_distance_from_left > 0.0 || signed_distance_from_right < 0.0) { obstacles_outside_of_path.push_back(o); obstacle_in_ego_path_times_.erase(obstacle_uuid_hex); @@ -482,7 +482,7 @@ std::optional RunOutModule::calcPredictedObstaclePose( const auto & p1 = predicted_path.at(i - 1); const auto & p2 = predicted_path.at(i); - const float ds = tier4_autoware_utils::calcDistance2d(p1, p2); + const float ds = autoware_universe_utils::calcDistance2d(p1, p2); const float dt = ds / velocity_mps; // apply linear interpolation between the predicted path points @@ -555,7 +555,7 @@ bool RunOutModule::checkCollisionWithCylinder( run_out_utils::createBoostPolyFromMsg(bounding_box_for_points); // check collision with 2d polygon - std::vector collision_points_bg; + std::vector collision_points_bg; bg::intersection(vehicle_polygon, bg_bounding_box_for_points, collision_points_bg); // no collision detected @@ -568,7 +568,7 @@ bool RunOutModule::checkCollisionWithCylinder( debug_ptr_->pushCollisionObstaclePolygons(bounding_box_for_points); for (const auto & p : collision_points_bg) { const auto p_msg = - tier4_autoware_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + autoware_universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); collision_points.emplace_back(p_msg); } @@ -581,30 +581,30 @@ std::vector RunOutModule::createBoundingBoxForRangedP const PoseWithRange & pose_with_range, const float x_offset, const float y_offset) const { const auto dist_p1_p2 = - tier4_autoware_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); + autoware_universe_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); geometry_msgs::msg::Pose p_min_to_p_max; if (dist_p1_p2 < std::numeric_limits::epsilon()) { // can't calculate the angle if two points are the same p_min_to_p_max = pose_with_range.pose_min; } else { - const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle( + const auto azimuth_angle = autoware_universe_utils::calcAzimuthAngle( pose_with_range.pose_min.position, pose_with_range.pose_max.position); p_min_to_p_max.position = pose_with_range.pose_min.position; - p_min_to_p_max.orientation = tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); + p_min_to_p_max.orientation = autoware_universe_utils::createQuaternionFromYaw(azimuth_angle); } std::vector poly; poly.emplace_back( - tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, y_offset, 0.0) + autoware_universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, y_offset, 0.0) .position); poly.emplace_back( - tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, -y_offset, 0.0) + autoware_universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, -y_offset, 0.0) .position); poly.emplace_back( - tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, -x_offset, -y_offset, 0.0).position); + autoware_universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, -y_offset, 0.0).position); poly.emplace_back( - tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, -x_offset, y_offset, 0.0).position); + autoware_universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, y_offset, 0.0).position); return poly; } @@ -620,7 +620,7 @@ bool RunOutModule::checkCollisionWithBoundingBox( const auto bg_bounding_box = run_out_utils::createBoostPolyFromMsg(bounding_box); // check collision with 2d polygon - std::vector collision_points_bg; + std::vector collision_points_bg; bg::intersection(vehicle_polygon, bg_bounding_box, collision_points_bg); // no collision detected @@ -633,7 +633,7 @@ bool RunOutModule::checkCollisionWithBoundingBox( debug_ptr_->pushCollisionObstaclePolygons(bounding_box); for (const auto & p : collision_points_bg) { const auto p_msg = - tier4_autoware_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + autoware_universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); collision_points.emplace_back(p_msg); } @@ -678,7 +678,7 @@ std::optional RunOutModule::calcStopPoint( // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; @@ -736,7 +736,7 @@ std::optional RunOutModule::calcStopPoint( // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; @@ -872,7 +872,7 @@ void RunOutModule::insertApproachingVelocity( } // debug - debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_universe_utils::calcOffsetPose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); const auto nearest_seg_idx_stop = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp index ed745cbb92679..448fdce82028f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -31,8 +31,8 @@ #else #include #endif +#include #include -#include namespace autoware::behavior_velocity_planner { namespace run_out_utils @@ -101,13 +101,15 @@ std::vector findLateralSameSidePoints( const std::vector & points, const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) { - const auto signed_deviation = tier4_autoware_utils::calcLateralDeviation(base_pose, target_point); + const auto signed_deviation = + autoware_universe_utils::calcLateralDeviation(base_pose, target_point); RCLCPP_DEBUG_STREAM( rclcpp::get_logger("findLateralSameSidePoints"), "signed dev of target: " << signed_deviation); std::vector same_side_points; for (const auto & p : points) { - const auto signed_deviation_of_point = tier4_autoware_utils::calcLateralDeviation(base_pose, p); + const auto signed_deviation_of_point = + autoware_universe_utils::calcLateralDeviation(base_pose, p); RCLCPP_DEBUG_STREAM( rclcpp::get_logger("findLateralSameSidePoints"), "signed dev of point: " << signed_deviation_of_point); @@ -125,7 +127,7 @@ std::vector findLateralSameSidePoints( bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) { - if (tier4_autoware_utils::calcDistance2d(p1, p2) < std::numeric_limits::epsilon()) { + if (autoware_universe_utils::calcDistance2d(p1, p2) < std::numeric_limits::epsilon()) { return true; } @@ -182,15 +184,15 @@ bool pathIntersectsEgoCutLine( { if (path.size() < 2) return false; const auto p1 = - tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position; + autoware_universe_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position; const auto p2 = - tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, -half_line_length, 0.0).position; + autoware_universe_utils::calcOffsetPose(ego_pose, 0.0, -half_line_length, 0.0).position; ego_cut_line = {p1, p2}; for (size_t i = 1; i < path.size(); ++i) { const auto & p3 = path.at(i).position; const auto & p4 = path.at(i - 1).position; - const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + const auto intersection = autoware_universe_utils::intersect(p1, p2, p3, p4); if (intersection.has_value()) { return true; } @@ -253,7 +255,7 @@ PathPointsWithLaneId decimatePathPoints( for (size_t i = 1; i < input_path_points.size(); i++) { const auto p1 = input_path_points.at(i - 1); const auto p2 = input_path_points.at(i); - const auto dist = tier4_autoware_utils::calcDistance2d(p1, p2); + const auto dist = autoware_universe_utils::calcDistance2d(p1, p2); dist_sum += dist; if (dist_sum > step) { @@ -281,7 +283,8 @@ PathWithLaneId trimPathFromSelfPose( output.points.push_back(input.points.at(i)); if (i != nearest_idx) { - dist_sum += tier4_autoware_utils::calcDistance2d(input.points.at(i - 1), input.points.at(i)); + dist_sum += + autoware_universe_utils::calcDistance2d(input.points.at(i - 1), input.points.at(i)); } if (dist_sum > trim_distance) { @@ -325,7 +328,7 @@ PathPointWithLaneId createExtendPathPoint( { PathPointWithLaneId extend_path_point = goal_point; extend_path_point.point.pose = - tier4_autoware_utils::calcOffsetPose(goal_point.point.pose, extend_distance, 0.0, 0.0); + autoware_universe_utils::calcOffsetPose(goal_point.point.pose, extend_distance, 0.0, 0.0); return extend_path_point; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 2a920c4b67a8e..0b79b7bbb9a7d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -15,7 +15,7 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -39,10 +39,10 @@ using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; -using tier4_autoware_utils::Box2d; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::Box2d; +using autoware_universe_utils::LineString2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp index 3d7679814cddd..e33d189ede54b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -15,9 +15,9 @@ #include "scene.hpp" #include +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -26,9 +26,9 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; namespace { @@ -105,7 +105,7 @@ motion_utils::VirtualWalls StopLineModule::createVirtualWalls() wall.text = "stopline"; wall.style = motion_utils::VirtualWallType::stop; wall.ns = std::to_string(module_id_) + "_"; - wall.pose = tier4_autoware_utils::calcOffsetPose( + wall.pose = autoware_universe_utils::calcOffsetPose( *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index 80f48fa65efaf..e510e6a32f7e3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include #include @@ -23,8 +23,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::TrafficSign; -using tier4_autoware_utils::getOrDeclareParameter; StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml index dc7e57d421d1f..49f3ab940d0df 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml @@ -16,6 +16,7 @@ autoware_behavior_velocity_planner_common autoware_planning_msgs autoware_route_handler + autoware_universe_utils eigen geometry_msgs lanelet2_extension @@ -25,7 +26,6 @@ rclcpp sensor_msgs tf2 - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 8aaf6be9bbfe9..768799382a27f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" +#include #include -#include #include @@ -28,7 +28,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; +using autoware_universe_utils::getOrDeclareParameter; TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 644e59e6b3011..b664186bb6bb5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -15,7 +15,7 @@ #include "scene.hpp" #include "motion_utils/trajectory/trajectory.hpp" -// #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +// #include "autoware/universe_utils/autoware_universe_utils.hpp" #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 4d011050a8675..d983a4ac27901 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -23,6 +23,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils eigen geometry_msgs lanelet2_extension @@ -32,7 +33,6 @@ tf2 tf2_eigen tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs traffic_light_utils visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp index 2b6d3cef9c364..d4af01842c786 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp @@ -15,9 +15,9 @@ #include "scene.hpp" #include +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -41,13 +41,13 @@ motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() wall.style = motion_utils::VirtualWallType::deadline; for (const auto & p : debug_data_.dead_line_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } wall.style = motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 12f972423aa40..375533fee48f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -15,7 +15,7 @@ #include "manager.hpp" #include -#include +#include #include @@ -26,8 +26,8 @@ #include namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::TrafficLight; -using tier4_autoware_utils::getOrDeclareParameter; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 7bec0b7fa9c01..b123d64d2eac6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_velocity_planner_common autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension @@ -27,7 +28,6 @@ nlohmann-json-dev pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 2f389d5d1104f..292680a68838b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -14,31 +14,31 @@ #include "scene.hpp" +#include +#include #include #include -#include -#include +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerOrientation; +using autoware_universe_utils::createMarkerPosition; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::toMsg; using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerPosition; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::toMsg; using namespace std::literals::string_literals; namespace autoware::behavior_velocity_planner { namespace { -[[maybe_unused]] tier4_autoware_utils::LinearRing3d createCircle( - const tier4_autoware_utils::Point3d & p, const double radius, const size_t num_points = 50) +[[maybe_unused]] autoware_universe_utils::LinearRing3d createCircle( + const autoware_universe_utils::Point3d & p, const double radius, const size_t num_points = 50) { - tier4_autoware_utils::LinearRing3d ring; // clockwise and closed + autoware_universe_utils::LinearRing3d ring; // clockwise and closed for (size_t i = 0; i < num_points; ++i) { - const double theta = i * (2 * tier4_autoware_utils::pi / num_points); + const double theta = i * (2 * autoware_universe_utils::pi / num_points); const double x = p.x() + radius * std::sin(theta); const double y = p.y() + radius * std::cos(theta); ring.emplace_back(x, y, p.z()); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 75d40f99a0f65..1009acaae768b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -14,12 +14,12 @@ #include "manager.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include +#include +#include #include -#include -#include #include @@ -33,8 +33,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::autoware::VirtualTrafficLight; -using tier4_autoware_utils::getOrDeclareParameter; namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) @@ -49,7 +49,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); p.hold_stop_margin_distance = getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); - p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad( + p.max_yaw_deviation_rad = autoware_universe_utils::deg2rad( getOrDeclareParameter(node, ns + ".max_yaw_deviation_deg")); p.check_timeout_after_stop_line = getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); @@ -59,10 +59,10 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const tier4_planning_msgs::msg::PathWithLaneId & path) { - tier4_autoware_utils::LineString2d ego_path_linestring; + autoware_universe_utils::LineString2d ego_path_linestring; for (const auto & path_point : path.points) { ego_path_linestring.push_back( - tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d()); + autoware_universe_utils::fromMsg(path_point.point.pose.position).to_2d()); } for (const auto & m : planning_utils::getRegElemMapOnPath( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 70c8fe731761f..bb398d9610685 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { namespace { -using tier4_autoware_utils::calcDistance2d; +using autoware_universe_utils::calcDistance2d; struct SegmentIndexWithPoint { @@ -46,16 +46,17 @@ tier4_v2x_msgs::msg::KeyValue createKeyValue(const std::string & key, const std: return tier4_v2x_msgs::build().key(key).value(value); } -tier4_autoware_utils::LineString3d toAutowarePoints(const lanelet::ConstLineString3d & line_string) +autoware_universe_utils::LineString3d toAutowarePoints( + const lanelet::ConstLineString3d & line_string) { - tier4_autoware_utils::LineString3d output; + autoware_universe_utils::LineString3d output; for (const auto & p : line_string) { output.emplace_back(p.x(), p.y(), p.z()); } return output; } -std::optional toAutowarePoints( +std::optional toAutowarePoints( const lanelet::Optional & line_string) { if (!line_string) { @@ -64,27 +65,28 @@ std::optional toAutowarePoints( return toAutowarePoints(*line_string); } -std::vector toAutowarePoints( +std::vector toAutowarePoints( const lanelet::ConstLineStrings3d & line_strings) { - std::vector output; + std::vector output; for (const auto & line_string : line_strings) { output.emplace_back(toAutowarePoints(line_string)); } return output; } -[[maybe_unused]] tier4_autoware_utils::LineString2d to_2d( - const tier4_autoware_utils::LineString3d & line_string) +[[maybe_unused]] autoware_universe_utils::LineString2d to_2d( + const autoware_universe_utils::LineString3d & line_string) { - tier4_autoware_utils::LineString2d output; + autoware_universe_utils::LineString2d output; for (const auto & p : line_string) { output.emplace_back(p.x(), p.y()); } return output; } -tier4_autoware_utils::Point3d calcCenter(const tier4_autoware_utils::LineString3d & line_string) +autoware_universe_utils::Point3d calcCenter( + const autoware_universe_utils::LineString3d & line_string) { const auto p1 = line_string.front(); const auto p2 = line_string.back(); @@ -95,10 +97,10 @@ tier4_autoware_utils::Point3d calcCenter(const tier4_autoware_utils::LineString3 geometry_msgs::msg::Pose calcHeadPose( const geometry_msgs::msg::Pose & base_link_pose, const double base_link_to_front) { - return tier4_autoware_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); + return autoware_universe_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); } -geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point3d & p) +geometry_msgs::msg::Point convertToGeomPoint(const autoware_universe_utils::Point3d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -109,7 +111,7 @@ geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point3d template std::optional findLastCollisionBeforeEndLine( - const T & points, const tier4_autoware_utils::LineString3d & target_line, + const T & points, const autoware_universe_utils::LineString3d & target_line, const size_t end_line_idx) { const auto target_line_p1 = convertToGeomPoint(target_line.at(0)); @@ -117,8 +119,8 @@ std::optional findLastCollisionBeforeEndLine( for (size_t i = end_line_idx; 0 < i; --i) { // NOTE: size_t can be used since it will not be negative. - const auto & p1 = tier4_autoware_utils::getPoint(points.at(i)); - const auto & p2 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto & p1 = autoware_universe_utils::getPoint(points.at(i)); + const auto & p2 = autoware_universe_utils::getPoint(points.at(i - 1)); const auto collision_point = arc_lane_utils::checkCollision(p1, p2, target_line_p1, target_line_p2); @@ -132,7 +134,7 @@ std::optional findLastCollisionBeforeEndLine( template std::optional findLastCollisionBeforeEndLine( - const T & points, const std::vector & lines, + const T & points, const std::vector & lines, const size_t end_line_idx) { for (const auto & line : lines) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index cccc52b92293c..5ffc583067f09 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/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 @@ -48,10 +48,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface std::string instrument_type{}; std::string instrument_id{}; std::vector custom_tags{}; - tier4_autoware_utils::Point3d instrument_center{}; - std::optional stop_line{}; - tier4_autoware_utils::LineString3d start_line{}; - std::vector end_lines{}; + autoware_universe_utils::Point3d instrument_center{}; + std::optional stop_line{}; + autoware_universe_utils::LineString3d start_line{}; + std::vector end_lines{}; }; struct ModuleData diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml index 544cb9fb1b84f..27d31f840524b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml @@ -21,6 +21,7 @@ autoware_behavior_velocity_planner_common autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension @@ -28,7 +29,6 @@ motion_utils pluginlib rclcpp - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp index fdc29f3b4f2dd..d95b3c54ae597 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp @@ -15,24 +15,24 @@ #include "scene_walkway.hpp" #include +#include +#include #include #include -#include -#include #include namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using motion_utils::createSlowDownVirtualWallMarker; using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; namespace diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index 0ecd7005e4267..821e37ae8c618 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -15,7 +15,7 @@ #include "manager.hpp" #include -#include +#include #include #include @@ -26,8 +26,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::autoware::Crosswalk; -using tier4_autoware_utils::getOrDeclareParameter; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index ec295913504cf..7b008d9541553 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -22,11 +22,11 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::getPose; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestSegmentIndex; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::getPose; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml index f728167f09f23..e0230fb2fb213 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml @@ -22,6 +22,7 @@ autoware_behavior_velocity_planner_common autoware_planning_msgs autoware_route_handler + autoware_universe_utils eigen geometry_msgs lanelet2_extension @@ -31,7 +32,6 @@ rclcpp sensor_msgs tf2 - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp index ff80faf06bbc4..33a25c82429ef 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -15,19 +15,19 @@ #include "scene.hpp" #include +#include +#include #include -#include -#include #include namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using visualization_msgs::msg::Marker; namespace diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/manager.cpp index e4c3eb659362e..99b9dcc366eb4 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -15,13 +15,13 @@ #include "manager.hpp" #include -#include +#include #include namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; +using autoware_universe_utils::getOrDeclareParameter; NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp index 7fdbde77754e9..ad4c6fc9cf637 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include @@ -23,7 +23,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::createPoint; +using autoware_universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, @@ -154,7 +154,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR } geometry_msgs::msg::Point stop_point = - tier4_autoware_utils::getPoint(path->points.at(target_point_idx).point); + autoware_universe_utils::getPoint(path->points.at(target_point_idx).point); const auto & op_stop_pose = planning_utils::insertStopPoint(stop_point, target_segment_idx, *path); @@ -215,7 +215,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - const auto & stop_pose = tier4_autoware_utils::getPose(path->points.at(0)); + const auto & stop_pose = autoware_universe_utils::getPose(path->points.at(0)); stop_factor.stop_pose = stop_pose; stop_factor.stop_factor_points.push_back(current_point); planning_utils::appendStopReason(stop_factor, stop_reason); diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp index 2b3e26ecaa36d..afa4a09560219 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -32,8 +32,8 @@ using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; using Line = bg::model::linestring; +using autoware_universe_utils::createPoint; using motion_utils::calcSignedArcLength; -using tier4_autoware_utils::createPoint; PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLanePolygon( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml index 491e8704bef41..73d7582131111 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml @@ -18,6 +18,7 @@ autoware_behavior_velocity_planner_common autoware_planning_msgs autoware_route_handler + autoware_universe_utils eigen geometry_msgs lanelet2_extension @@ -27,7 +28,6 @@ rclcpp sensor_msgs tf2 - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp index c9818219fe671..3858490c19f57 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp @@ -15,23 +15,23 @@ #include "scene.hpp" #include +#include +#include #include #include #include -#include -#include #include namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using motion_utils::createSlowDownVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; namespace @@ -104,7 +104,7 @@ motion_utils::VirtualWalls SpeedBumpModule::createVirtualWalls() wall.ns = std::to_string(module_id_) + "_"; wall.style = motion_utils::VirtualWallType::slowdown; for (const auto & p : debug_data_.slow_start_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp index 87d72cc326c7e..2c753f6dd2fd2 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp @@ -15,9 +15,9 @@ #include "manager.hpp" #include +#include #include #include -#include #include @@ -30,8 +30,8 @@ namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::getOrDeclareParameter; using lanelet::autoware::SpeedBump; -using tier4_autoware_utils::getOrDeclareParameter; SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp index 0ca4e0731b155..5c4f8a39c0217 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp @@ -14,16 +14,16 @@ #include "scene.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include namespace autoware::behavior_velocity_planner { +using autoware_universe_utils::createPoint; using motion_utils::calcSignedArcLength; -using tier4_autoware_utils::createPoint; using geometry_msgs::msg::Point32; diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp index cbe0b057c0c7e..41cfc911e2985 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp @@ -18,7 +18,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include @@ -40,11 +40,11 @@ using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; using Line = bg::model::linestring; +using autoware_universe_utils::createPoint; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; -using tier4_autoware_utils::createPoint; PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index c0eca8028cb95..da2ff14c76813 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -16,6 +16,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs libboost-dev @@ -23,7 +24,6 @@ pluginlib rclcpp tf2 - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp index c79c6c32802a3..d5873e5f462a1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -14,9 +14,9 @@ #include "collision.hpp" +#include +#include #include -#include -#include #include @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop std::optional find_closest_collision_point( const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose, - const tier4_autoware_utils::Polygon2d & object_footprint) + const autoware_universe_utils::Polygon2d & object_footprint) { std::optional closest_collision_point; auto closest_dist = std::numeric_limits::max(); @@ -40,10 +40,10 @@ std::optional find_closest_collision_point( const auto traj_idx = rough_collision.second; const auto & ego_footprint = ego_data.trajectory_footprints[traj_idx]; const auto & ego_pose = ego_data.trajectory[traj_idx].pose; - const auto angle_diff = tier4_autoware_utils::normalizeRadian( + const auto angle_diff = autoware_universe_utils::normalizeRadian( tf2::getYaw(ego_pose.orientation) - tf2::getYaw(object_pose.orientation)); if (std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { // TODO(Maxime): make this angle a parameter - tier4_autoware_utils::MultiPoint2d collision_points; + autoware_universe_utils::MultiPoint2d collision_points; boost::geometry::intersection( object_footprint.outer(), ego_footprint.outer(), collision_points); for (const auto & coll_p : collision_points) { @@ -63,7 +63,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, - const tier4_autoware_utils::MultiPolygon2d & object_forward_footprints) + const autoware_universe_utils::MultiPolygon2d & object_forward_footprints) { std::vector collisions; for (auto object_idx = 0UL; object_idx < objects.size(); ++object_idx) { @@ -72,7 +72,7 @@ std::vector find_collisions( const auto collision = find_closest_collision_point(ego_data, object_pose, object_footprint); if (collision) { Collision c; - c.object_uuid = tier4_autoware_utils::toHexString(objects[object_idx].object_id); + c.object_uuid = autoware_universe_utils::toHexString(objects[object_idx].object_id); c.point = *collision; collisions.push_back(c); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 6b995d715c190..78413d2faacad 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -30,7 +30,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop /// @return the collision point closest to ego (if any) std::optional find_closest_collision_point( const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose, - const tier4_autoware_utils::Polygon2d & object_footprint); + const autoware_universe_utils::Polygon2d & object_footprint); /// @brief find the earliest collision along the ego trajectory /// @param [in] ego_data ego data including its trajectory and footprint @@ -40,7 +40,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, - const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); + const autoware_universe_utils::MultiPolygon2d & obstacle_forward_footprints); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp index bfca43edaeac9..eaa148111dc49 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -16,7 +16,7 @@ #include "types.hpp" -#include +#include #include @@ -59,8 +59,8 @@ std::vector make_collision_markers( marker.ns = ns; marker.id = 0; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.5); - marker.color = tier4_autoware_utils::createMarkerColor(0.6, 0.0, 0.6, 1.0); + marker.scale = autoware_universe_utils::createMarkerScale(0.2, 0.2, 0.5); + marker.color = autoware_universe_utils::createMarkerColor(0.6, 0.0, 0.6, 1.0); for (const auto & [object_uuid, decision] : object_map) { marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker.text = object_uuid.substr(0, 5) + "\n"; @@ -85,7 +85,8 @@ std::vector make_collision_markers( } std::vector make_polygon_markers( - const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z) + const autoware_universe_utils::MultiPolygon2d & footprints, const std::string & ns, + const double z) { std::vector markers; visualization_msgs::msg::Marker marker; @@ -95,8 +96,8 @@ std::vector make_polygon_markers( marker.id = 0; marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); - marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); + marker.scale = autoware_universe_utils::createMarkerScale(0.1, 0.1, 0.1); + marker.color = autoware_universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); for (const auto & footprint : footprints) { marker.points.clear(); for (const auto & p : footprint.outer()) { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp index c708f0c86a983..35656acb0b267 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -18,8 +18,8 @@ #include "object_stop_decision.hpp" #include "types.hpp" +#include #include -#include #include #include @@ -39,7 +39,8 @@ std::vector make_collision_markers( const ObjectStopDecisionMap & object_map, const std::string & ns, const double z, const rclcpp::Time & now); std::vector make_polygon_markers( - const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); + const autoware_universe_utils::MultiPolygon2d & footprints, const std::string & ns, + const double z); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop::debug #endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 4d4b9b32a0f32..ee4dbb01bd71d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -21,12 +21,12 @@ #include "object_stop_decision.hpp" #include "types.hpp" +#include +#include +#include +#include #include #include -#include -#include -#include -#include #include #include @@ -49,7 +49,7 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - using tier4_autoware_utils::getOrDeclareParameter; + using autoware_universe_utils::getOrDeclareParameter; auto & p = params_; p.extra_object_width = getOrDeclareParameter(node, ns_ + ".extra_object_width"); p.minimum_object_velocity = getOrDeclareParameter(node, ns_ + ".minimum_object_velocity"); @@ -72,7 +72,7 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo void DynamicObstacleStopModule::update_parameters(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto & p = params_; updateParam(parameters, ns_ + ".extra_object_width", p.extra_object_width); updateParam(parameters, ns_ + ".minimum_object_velocity", p.minimum_object_velocity); @@ -95,7 +95,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.reset_data(); if (ego_trajectory_points.size() < 2) return result; - tier4_autoware_utils::StopWatch stopwatch; + autoware_universe_utils::StopWatch stopwatch; stopwatch.tic(); stopwatch.tic("preprocessing"); dynamic_obstacle_stop::EgoData ego_data; @@ -156,7 +156,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( result.stop_points.push_back(stop_pose->position); const auto stop_pose_reached = planner_data->current_odometry.twist.twist.linear.x < 1e-3 && - tier4_autoware_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; + autoware_universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; velocity_factor_interface_.set( ego_trajectory_points, ego_data.pose, *stop_pose, stop_pose_reached ? motion_utils::VelocityFactor::STOPPED diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index d5b0ad3d117d2..b4f2bdc55cea1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -14,7 +14,7 @@ #include "footprint.hpp" -#include +#include #include @@ -28,11 +28,11 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { -tier4_autoware_utils::MultiPolygon2d make_forward_footprints( +autoware_universe_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, const PlannerParam & params, const double hysteresis) { - tier4_autoware_utils::MultiPolygon2d forward_footprints; + autoware_universe_utils::MultiPolygon2d forward_footprints; for (const auto & obstacle : obstacles) forward_footprints.push_back(project_to_pose( make_forward_footprint(obstacle, params, hysteresis), @@ -40,7 +40,7 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( return forward_footprints; } -tier4_autoware_utils::Polygon2d make_forward_footprint( +autoware_universe_utils::Polygon2d make_forward_footprint( const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis) { @@ -49,7 +49,7 @@ tier4_autoware_utils::Polygon2d make_forward_footprint( shape.x / 2.0 + obstacle.kinematics.initial_twist_with_covariance.twist.linear.x * params.time_horizon; const auto lateral_offset = (shape.y + params.extra_object_width) / 2.0 + hysteresis; - return tier4_autoware_utils::Polygon2d{ + return autoware_universe_utils::Polygon2d{ {{longitudinal_offset, -lateral_offset}, {longitudinal_offset, lateral_offset}, {shape.x / 2.0, lateral_offset}, @@ -58,12 +58,12 @@ tier4_autoware_utils::Polygon2d make_forward_footprint( {}}; } -tier4_autoware_utils::Polygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +autoware_universe_utils::Polygon2d project_to_pose( + const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); - tier4_autoware_utils::Polygon2d footprint; + const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); + autoware_universe_utils::Polygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.outer().emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); return footprint; @@ -72,10 +72,10 @@ tier4_autoware_utils::Polygon2d project_to_pose( void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) { for (const auto & p : ego_data.trajectory) - ego_data.trajectory_footprints.push_back(tier4_autoware_utils::toFootprint( + ego_data.trajectory_footprints.push_back(autoware_universe_utils::toFootprint( p.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { - const auto box = boost::geometry::return_envelope( + const auto box = boost::geometry::return_envelope( ego_data.trajectory_footprints[i]); ego_data.rtree.insert(std::make_pair(box, i)); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index 8ceb19d679ea0..85a736869a7f8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop /// @param [in] params parameters used to create the footprint /// @param [in] hysteresis [m] extra lateral distance to add to the footprints /// @return forward footprint of the obstacle -tier4_autoware_utils::MultiPolygon2d make_forward_footprints( +autoware_universe_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, const PlannerParam & params, const double hysteresis); /// @brief create the footprint of the given obstacle and its projection over a fixed time horizon @@ -37,15 +37,15 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( /// @param [in] params parameters used to create the footprint /// @param [in] hysteresis [m] extra lateral distance to add to the footprint /// @return forward footprint of the obstacle -tier4_autoware_utils::Polygon2d make_forward_footprint( +autoware_universe_utils::Polygon2d make_forward_footprint( const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose -tier4_autoware_utils::Polygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +autoware_universe_utils::Polygon2d project_to_pose( + const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); /// @brief create the rtree indexing the ego footprint along the trajectory /// @param [inout] ego_data ego data with its trajectory and the rtree to populate /// @param [in] params parameters diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 0bb3c90ad262b..223e7487b36e9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -62,7 +62,7 @@ std::vector filter_predicted_obj const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose; const auto o_yaw = tf2::getYaw(o_pose.orientation); const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation); - const auto yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(o_yaw - ego_yaw)); + const auto yaw_diff = std::abs(autoware_universe_utils::normalizeRadian(o_yaw - ego_yaw)); const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4; const auto collision_distance_threshold = params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp index 93170bead32e8..0a286112010a7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -15,8 +15,8 @@ #ifndef TYPES_HPP_ #define TYPES_HPP_ +#include #include -#include #include #include @@ -32,7 +32,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { using TrajectoryPoints = std::vector; -using BoxIndexPair = std::pair; +using BoxIndexPair = std::pair; using Rtree = boost::geometry::index::rtree>; /// @brief parameters for the "out of lane" module @@ -57,7 +57,7 @@ struct EgoData size_t first_trajectory_idx{}; double longitudinal_offset_to_first_trajectory_idx; // [m] geometry_msgs::msg::Pose pose; - tier4_autoware_utils::MultiPolygon2d trajectory_footprints; + autoware_universe_utils::MultiPolygon2d trajectory_footprints; Rtree rtree; std::optional earliest_stop_pose; }; @@ -65,9 +65,9 @@ struct EgoData /// @brief debug data struct DebugData { - tier4_autoware_utils::MultiPolygon2d obstacle_footprints{}; + autoware_universe_utils::MultiPolygon2d obstacle_footprints{}; size_t prev_dynamic_obstacles_nb{}; - tier4_autoware_utils::MultiPolygon2d ego_footprints{}; + autoware_universe_utils::MultiPolygon2d ego_footprints{}; size_t prev_ego_footprints_nb{}; std::optional stop_pose{}; size_t prev_collisions_nb{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml index ff30193cafee1..7b6f3c6a49c6c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml @@ -16,6 +16,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils eigen grid_map_msgs @@ -31,7 +32,6 @@ sensor_msgs tf2_eigen tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index f67ca131ee91c..a281826b64ed7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -17,7 +17,7 @@ #include "distance.hpp" #include "forward_projection.hpp" -#include +#include #include #include @@ -129,7 +129,7 @@ std::vector calculate_slowd if (i > 0) { const auto & prev_point = trajectory[i - 1]; time += static_cast( - tier4_autoware_utils::calcDistance2d(prev_point, trajectory_point) / + autoware_universe_utils::calcDistance2d(prev_point, trajectory_point) / prev_point.longitudinal_velocity_mps); } // First linestring is used to calculate distance diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index f845cf040dd64..74b027efd3d1a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -21,14 +21,14 @@ #include "parameters.hpp" #include "trajectory_preprocessing.hpp" +#include +#include #include #include #include #include #include #include -#include -#include #include @@ -132,7 +132,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { - tier4_autoware_utils::StopWatch stopwatch; + autoware_universe_utils::StopWatch stopwatch; stopwatch.tic(); VelocityPlanningResult result; stopwatch.tic("preprocessing"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp index 796516253f680..e06ca4fffae28 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp @@ -21,10 +21,10 @@ #include #include +#include +#include +#include #include -#include -#include -#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index 5276761ca3ee0..7e4704ba6818c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -18,7 +18,7 @@ #include "parameters.hpp" #include "types.hpp" -#include +#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp index 3360d7e201231..eda2e2624ca4a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp @@ -15,8 +15,8 @@ #ifndef POINTCLOUD_UTILS_HPP_ #define POINTCLOUD_UTILS_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "obstacles.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "types.hpp" #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index 73cede754b748..430c19c3d35ff 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp @@ -14,7 +14,7 @@ #include "trajectory_preprocessing.hpp" -#include +#include #include @@ -31,7 +31,7 @@ size_t calculateStartIndex( auto dist = 0.0; auto idx = ego_idx; while (idx + 1 < trajectory.size() && dist < start_distance) { - dist += tier4_autoware_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); + dist += autoware_universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); ++idx; } return idx; @@ -46,7 +46,7 @@ size_t calculateEndIndex( auto idx = start_idx; while (idx + 1 < trajectory.size() && length < max_length && duration < max_duration) { const auto length_d = - tier4_autoware_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); + autoware_universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); length += length_d; if (trajectory[idx].longitudinal_velocity_mps > 0.0) duration += length_d / trajectory[idx].longitudinal_velocity_mps; @@ -74,7 +74,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b for (auto i = 1ul; i < trajectory.size(); ++i) { const auto & prev_point = trajectory[i - 1]; auto & point = trajectory[i]; - const auto dt = tier4_autoware_utils::calcDistance2d(prev_point, point) / + const auto dt = autoware_universe_utils::calcDistance2d(prev_point, point) / prev_point.longitudinal_velocity_mps; t += dt; const auto heading = tf2::getYaw(point.pose.orientation); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp index c24b1aff66eec..dcab3d8d1bcac 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp @@ -15,7 +15,7 @@ #ifndef TYPES_HPP_ #define TYPES_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include @@ -35,13 +35,13 @@ using nav_msgs::msg::OccupancyGrid; using PointCloud = pcl::PointCloud; using TrajectoryPoints = std::vector; -using point_t = tier4_autoware_utils::Point2d; -using multipoint_t = tier4_autoware_utils::MultiPoint2d; -using polygon_t = tier4_autoware_utils::Polygon2d; -using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; -using segment_t = tier4_autoware_utils::Segment2d; -using linestring_t = tier4_autoware_utils::LineString2d; -using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; +using point_t = autoware_universe_utils::Point2d; +using multipoint_t = autoware_universe_utils::MultiPoint2d; +using polygon_t = autoware_universe_utils::Polygon2d; +using multi_polygon_t = autoware_universe_utils::MultiPolygon2d; +using segment_t = autoware_universe_utils::Segment2d; +using linestring_t = autoware_universe_utils::LineString2d; +using multi_linestring_t = autoware_universe_utils::MultiLineString2d; struct ObstacleMasks { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index 325f17d36a37d..0112de62f3950 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -15,7 +15,7 @@ #include "../src/distance.hpp" #include "../src/obstacles.hpp" #include "../src/types.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -400,7 +400,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; object1.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; object1.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(0.0); + autoware_universe_utils::createQuaternionFromYaw(0.0); object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; @@ -427,7 +427,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_pose_with_covariance.pose.position.x = 10.0; object2.kinematics.initial_pose_with_covariance.pose.position.y = 10.0; object2.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(M_PI_2); + autoware_universe_utils::createQuaternionFromYaw(M_PI_2); object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp index 96dd5932806b2..0b8aad36580e6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp @@ -14,7 +14,7 @@ #include "../src/forward_projection.hpp" #include "../src/types.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #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 8dab409b48ad5..df17c10cd4184 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 @@ -18,6 +18,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension @@ -26,7 +27,6 @@ pluginlib rclcpp tf2 - tier4_autoware_utils tier4_planning_msgs traffic_light_utils visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 157d545dcac0d..97df4e400e4df 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -39,7 +39,7 @@ bool can_decelerate( std::optional calculate_last_in_lane_pose( const EgoData & ego_data, const Slowdown & decision, - const tier4_autoware_utils::Polygon2d & footprint, + const autoware_universe_utils::Polygon2d & footprint, const std::optional & prev_slowdown_point, const PlannerParam & params) { const auto from_arc_length = diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 19ed066548d0f..47ea84dd74a06 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include #include @@ -42,7 +42,7 @@ bool can_decelerate( /// @return the last pose that is not out of lane (if found) std::optional calculate_last_in_lane_pose( const EgoData & ego_data, const Slowdown & decision, - const tier4_autoware_utils::Polygon2d & footprint, + const autoware_universe_utils::Polygon2d & footprint, const std::optional & prev_slowdown_point, const PlannerParam & params); /// @brief calculate the slowdown point to insert in the trajectory diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index efb957ea85691..e7554c449d264 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -16,7 +16,7 @@ #include "types.hpp" -#include +#include #include @@ -36,10 +36,10 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.id = 0; base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + base_marker.pose.position = autoware_universe_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = autoware_universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = autoware_universe_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); return base_marker; } void add_footprint_markers( @@ -52,7 +52,7 @@ void add_footprint_markers( debug_marker.points.clear(); for (const auto & p : f) debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + autoware_universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); debug_marker.points.push_back(debug_marker.points.front()); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; @@ -71,19 +71,19 @@ void add_current_overlap_marker( debug_marker.ns = "current_overlap"; debug_marker.points.clear(); for (const auto & p : current_footprint) - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z)); + debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition(p.x(), p.y(), z)); if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); if (current_overlapped_lanelets.empty()) - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); + debug_marker.color = autoware_universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); else - debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker.color = autoware_universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; for (const auto & ll : current_overlapped_lanelets) { debug_marker.points.clear(); for (const auto & p : ll.polygon3d()) debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); + autoware_universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); debug_marker.points.push_back(debug_marker.points.front()); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; @@ -106,7 +106,7 @@ void add_lanelet_markers( // add a small z offset to draw above the lanelet map for (const auto & p : ll.polygon3d()) debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); + autoware_universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); debug_marker.points.push_back(debug_marker.points.front()); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; @@ -123,20 +123,20 @@ void add_range_markers( { auto debug_marker = get_base_marker(); debug_marker.ns = "ranges"; - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); + debug_marker.color = autoware_universe_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); for (const auto & range : ranges) { debug_marker.points.clear(); debug_marker.points.push_back( trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( range.entering_point.x(), range.entering_point.y(), z)); for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); } - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( range.exiting_point.x(), range.exiting_point.y(), z)); debug_marker.points.push_back( trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); @@ -156,12 +156,12 @@ void add_decision_markers( debug_marker.action = debug_marker.ADD; debug_marker.id = 0; debug_marker.ns = "decisions"; - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); + debug_marker.color = autoware_universe_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); debug_marker.points.clear(); for (const auto & range : ranges) { debug_marker.type = debug_marker.LINE_STRIP; if (range.debug.decision) { - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + debug_marker.points.push_back(autoware_universe_utils::createMarkerPosition( range.entering_point.x(), range.entering_point.y(), z)); debug_marker.points.push_back( range.debug.object->kinematics.initial_pose_with_covariance.pose.position); @@ -210,14 +210,15 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data.prev_current_overlapped_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", - tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), + autoware_universe_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), debug_data.prev_trajectory_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", - tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data.prev_ignored_lanelets); + autoware_universe_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), + debug_data.prev_ignored_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data.other_lanelets, "other_lanelets", - tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data.prev_other_lanelets); + autoware_universe_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data.prev_other_lanelets); debug::add_range_markers( debug_marker_array, debug_data.ranges, debug_data.trajectory_points, debug_data.first_trajectory_idx, z, debug_data.prev_ranges); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp index 81610f750e753..7746f1d9ba793 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -14,10 +14,10 @@ #include "decisions.hpp" +#include +#include #include #include -#include -#include #include @@ -91,7 +91,7 @@ std::optional> object_time_to_range( unique_path_points, enter_segment_idx, enter_point); const auto enter_lat_dist = std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( + const auto enter_segment_length = autoware_universe_utils::calcDistance2d( unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); const auto enter_offset_ratio = enter_offset / enter_segment_length; const auto enter_time = @@ -105,7 +105,7 @@ std::optional> object_time_to_range( unique_path_points, exit_segment_idx, exit_point); const auto exit_lat_dist = std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( + const auto exit_segment_length = autoware_universe_utils::calcDistance2d( unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); const auto exit_time = @@ -314,7 +314,7 @@ bool should_not_enter( for (const auto & object : inputs.objects.objects) { RCLCPP_DEBUG( logger, "\t\t[%s] going at %2.2fm/s", - tier4_autoware_utils::toHexString(object.object_id).c_str(), + autoware_universe_utils::toHexString(object.object_id).c_str(), object.kinematics.initial_twist_with_covariance.twist.linear.x); if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index 77292a41b9cf1..ea50946a32267 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -50,7 +50,7 @@ void cut_predicted_path_beyond_line( auto cut_idx = stop_line_idx; double arc_length = 0; while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += tier4_autoware_utils::calcDistance2d( + arc_length += autoware_universe_utils::calcDistance2d( predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); --cut_idx; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index 824a847b17414..f411485a0750f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -14,7 +14,7 @@ #include "footprint.hpp" -#include +#include #include @@ -26,10 +26,10 @@ namespace autoware::motion_velocity_planner::out_of_lane { -tier4_autoware_utils::Polygon2d make_base_footprint( +autoware_universe_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset) { - tier4_autoware_utils::Polygon2d base_footprint; + autoware_universe_utils::Polygon2d base_footprint; const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; @@ -43,10 +43,10 @@ tier4_autoware_utils::Polygon2d make_base_footprint( } lanelet::BasicPolygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) + const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); @@ -66,14 +66,14 @@ std::vector calculate_trajectory_footprints( i < ego_data.trajectory_points.size() && length < range; ++i) { const auto & trajectory_pose = ego_data.trajectory_points[i].pose; const auto angle = tf2::getYaw(trajectory_pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back( p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); trajectory_footprints.push_back(footprint); if (i + 1 < ego_data.trajectory_points.size()) - length += tier4_autoware_utils::calcDistance2d( + length += autoware_universe_utils::calcDistance2d( trajectory_pose, ego_data.trajectory_points[i + 1].pose); } return trajectory_footprints; @@ -84,7 +84,7 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( { const auto base_footprint = make_base_footprint(params, ignore_offset); const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware_universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp index 7b80dd9618bb7..bbcdd7b84dd18 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include @@ -30,14 +30,14 @@ namespace out_of_lane /// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the /// footprint /// @return base ego footprint -tier4_autoware_utils::Polygon2d make_base_footprint( +autoware_universe_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset = false); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose lanelet::BasicPolygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); + const autoware_universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); /// @brief calculate the trajectory footprints /// @details the resulting polygon follows the format used by the lanelet library: clockwise order /// and implicit closing edge diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index 23dbf77f08151..60d627a4b8c5d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -14,7 +14,7 @@ #include "lanelets_selection.hpp" -#include +#include #include #include @@ -91,7 +91,7 @@ lanelet::ConstLanelets calculate_ignored_lanelets( lanelet::ConstLanelets ignored_lanelets; // ignore lanelets directly behind ego const auto behind = - tier4_autoware_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); + autoware_universe_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); const auto behind_lanelets = lanelet::geometry::findWithin2d( route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); 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 11640f9ad2c8e..d4f8e3a33e6e8 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 @@ -23,11 +23,11 @@ #include "overlapping_range.hpp" #include "types.hpp" +#include +#include +#include #include #include -#include -#include -#include #include @@ -59,7 +59,7 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { - using tier4_autoware_utils::getOrDeclareParameter; + using autoware_universe_utils::getOrDeclareParameter; auto & pp = params_; pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); @@ -109,7 +109,7 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) void OutOfLaneModule::update_parameters(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; auto & pp = params_; updateParam(parameters, ns_ + ".mode", pp.mode); updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); @@ -151,7 +151,7 @@ VelocityPlanningResult OutOfLaneModule::plan( const std::shared_ptr planner_data) { VelocityPlanningResult result; - tier4_autoware_utils::StopWatch stopwatch; + autoware_universe_utils::StopWatch stopwatch; stopwatch.tic(); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp index f89c620b75fb6..a1a722bb07f14 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -14,8 +14,8 @@ #include "overlapping_range.hpp" +#include #include -#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp index 07c8663ea21bc..bc5872db16e03 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 7c1d370ae0ced..2f29f9a8a8d1c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -19,6 +19,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_velocity_smoother eigen geometry_msgs @@ -26,7 +27,6 @@ libboost-dev motion_utils rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 26bbcce3ee2a4..7d272eedd4a70 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -21,6 +21,7 @@ autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils autoware_velocity_smoother diagnostic_msgs eigen @@ -37,7 +38,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 9e4816329a7cd..77caa70ce5626 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -14,14 +14,14 @@ #include "node.hpp" +#include +#include +#include #include #include #include #include #include -#include -#include -#include #include #include @@ -102,8 +102,9 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & set_param_callback_ = this->add_on_set_parameters_callback( std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void MotionVelocityPlannerNode::on_load_plugin( @@ -192,7 +193,7 @@ MotionVelocityPlannerNode::process_no_ground_pointcloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); - if (!pc.empty()) tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + if (!pc.empty()) autoware_universe_utils::transformPointCloud(pc, *pc_transformed, affine); return *pc_transformed; } @@ -385,7 +386,7 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; { std::unique_lock lk(mutex_); // for planner_manager_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 54ee64a7c861f..42c34a9a8c68d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -18,12 +18,12 @@ #include "planner_manager.hpp" #include +#include +#include +#include #include #include #include -#include -#include -#include #include #include @@ -62,23 +62,23 @@ class MotionVelocityPlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr sub_trajectory_; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::PredictedObjects> sub_predicted_objects_{this, "~/input/dynamic_objects"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ - this, "~/input/no_ground_pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()}; - tier4_autoware_utils::InterProcessPollingSubscriber + this, "~/input/no_ground_pointcloud", autoware_universe_utils::SingleDepthSensorQoS()}; + autoware_universe_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_acceleration_{this, "~/input/accel"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; rclcpp::Subscription::SharedPtr sub_lanelet_map_; @@ -138,9 +138,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware_planning_msgs::msg::Trajectory generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp index ef035b94054aa..54c12ed0ef543 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -33,7 +33,7 @@ #define EIGEN_MPL2_ONLY #include #include -#include +#include namespace motion_planning { @@ -108,10 +108,10 @@ class ObstacleStopPlannerDebugNode explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); ~ObstacleStopPlannerDebugNode() {} bool pushPolygon( - const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); bool pushPose(const Pose & pose, const PoseType & type); 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 d7fc83dbdf7ed..7bcaf44bbc443 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -15,19 +15,19 @@ #ifndef OBSTACLE_STOP_PLANNER__NODE_HPP_ #define OBSTACLE_STOP_PLANNER__NODE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/planner_data.hpp" -#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 #include @@ -81,9 +81,9 @@ using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::StopWatch; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using autoware_universe_utils::StopWatch; using tier4_debug_msgs::msg::BoolStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -315,9 +315,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node return ret; } - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace motion_planning 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 7b30ac61af46f..bdd2a0a403fcb 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 @@ -47,8 +47,8 @@ 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 autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 39195f2840b81..4d68dbfac2b80 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_universe_utils autoware_vehicle_info_utils diagnostic_msgs motion_utils @@ -39,7 +40,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 2931da5830a60..b2ccf896619de 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -30,7 +30,7 @@ #include #endif -#include +#include #include #include @@ -458,7 +458,7 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( object.kinematics.initial_twist_with_covariance.twist.linear.x); *velocity = - obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); + obj_vel_norm * std::cos(autoware_universe_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; } diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 031c7d79bf4c5..d105777b49f9f 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -14,9 +14,9 @@ #include "obstacle_stop_planner/debug_marker.hpp" +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -27,16 +27,16 @@ #include #include +using autoware_universe_utils::appendMarkerArray; +using autoware_universe_utils::calcOffsetPose; +using autoware_universe_utils::createDefaultMarker; +using autoware_universe_utils::createMarkerColor; +using autoware_universe_utils::createMarkerScale; +using autoware_universe_utils::createPoint; using motion_utils::createDeletedSlowDownVirtualWallMarker; using motion_utils::createDeletedStopVirtualWallMarker; using motion_utils::createSlowDownVirtualWallMarker; using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; namespace motion_planning { @@ -54,7 +54,7 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const autoware_universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; for (const auto & point : polygon.outer()) { @@ -100,7 +100,7 @@ bool ObstacleStopPlannerDebugNode::pushPolygon( } bool ObstacleStopPlannerDebugNode::pushPolyhedron( - const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 3a269d738e02d..245ed69eb04fc 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -41,16 +41,16 @@ namespace motion_planning { using autoware_perception_msgs::msg::PredictedObject; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::createPoint; +using autoware_universe_utils::getPoint; +using autoware_universe_utils::getPose; +using autoware_universe_utils::getRPY; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::getPoint; -using tier4_autoware_utils::getPose; -using tier4_autoware_utils::getRPY; ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_stop_planner", node_options) @@ -245,8 +245,9 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), createSubscriptionOptions(this)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -1557,8 +1558,8 @@ void ObstacleStopPlannerNode::filterObstacles( const double max_length = calcObstacleMaxLength(object.shape); const size_t seg_idx = motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); - const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1)); + const auto p_front = autoware_universe_utils::getPoint(traj.at(seg_idx)); + const auto p_back = autoware_universe_utils::getPoint(traj.at(seg_idx + 1)); const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 3e2ecca8ec4d7..ed1ee450140a5 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -14,10 +14,10 @@ #include "obstacle_stop_planner/planner_utils.hpp" +#include #include #include #include -#include #include @@ -34,11 +34,11 @@ namespace motion_planning using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::calcDistance2d; +using autoware_universe_utils::getRPY; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getRPY; std::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, @@ -325,40 +325,42 @@ void createOneStepPolygon( const double rear_overhang = vehicle_info.rear_overhang_m; { // base step - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) - .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + autoware_universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) + .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + autoware_universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); } { // next step - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) - .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + autoware_universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) + .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + autoware_universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); } - polygon = tier4_autoware_utils::isClockwise(polygon) + polygon = autoware_universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware_universe_utils::inverseClockwise(polygon); boost::geometry::convex_hull(polygon, hull_polygon); } @@ -550,7 +552,7 @@ double getNearestPointAndDistanceForPredictedObject( bool is_init = false; for (const auto & p : points.poses) { - double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); + double norm = autoware_universe_utils::calcDistance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; *nearest_collision_point = p.position; @@ -625,9 +627,9 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = tier4_autoware_utils::isClockwise(object_polygon) + object_polygon = autoware_universe_utils::isClockwise(object_polygon) ? object_polygon - : tier4_autoware_utils::inverseClockwise(object_polygon); + : autoware_universe_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -648,9 +650,9 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = tier4_autoware_utils::isClockwise(object_polygon) + object_polygon = autoware_universe_utils::isClockwise(object_polygon) ? object_polygon - : tier4_autoware_utils::inverseClockwise(object_polygon); + : autoware_universe_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -668,9 +670,9 @@ Polygon2d convertPolygonObjectToGeometryPolygon( object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); } object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = tier4_autoware_utils::isClockwise(object_polygon) + object_polygon = autoware_universe_utils::isClockwise(object_polygon) ? object_polygon - : tier4_autoware_utils::inverseClockwise(object_polygon); + : autoware_universe_utils::inverseClockwise(object_polygon); return object_polygon; } @@ -690,7 +692,7 @@ std::optional getObstacleFromUuid( bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) { - const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z; + const auto yaw = autoware_universe_utils::getRPY(ego_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); const Eigen::Vector2d obstacle_vec( obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); diff --git a/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp index 88eb2f3244e40..0dd4c74ddbb06 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ #define AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_frenet_planner/structures.hpp" #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index ba3156c6b085d..0aae35126e8ab 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -176,7 +176,7 @@ void calculateCartesian( pose.position.x = it->x(); pose.position.y = it->y(); pose.position.z = 0.0; - pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + pose.orientation = autoware_universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); path.poses.push_back(pose); } path.yaws.push_back(path.yaws.back()); diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp index 5a8183d8e5926..9fdf7191a911d 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ #define AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/structures.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" -#include +#include #include #include @@ -80,7 +80,7 @@ struct TimeKeeper std::string accumulated_msg = "\n"; std::stringstream latest_stream; - tier4_autoware_utils::StopWatch< + autoware_universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -89,8 +89,8 @@ struct DebugData { std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; - std::vector obstacles{}; - std::vector footprints{}; + std::vector obstacles{}; + std::vector footprints{}; }; struct TrajectoryParam @@ -103,7 +103,7 @@ struct TrajectoryParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; // common updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); @@ -123,7 +123,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } 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 780c71b1ab9cd..ceab5229b36d4 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 @@ -22,8 +22,8 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" +#include #include -#include #include #include @@ -62,8 +62,9 @@ class PathSampler : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber objects_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber odom_sub_{ + this, "~/input/odometry"}; + autoware_universe_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; // debug publisher 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 b6c63eeffed18..004739c7c9ecf 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 @@ -42,8 +42,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = tier4_autoware_utils::getPoint(t1); - const auto p2 = tier4_autoware_utils::getPoint(t2); + const auto p1 = autoware_universe_utils::getPoint(t1); + const auto p2 = autoware_universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index 4f346ae910f44..70909231de176 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -52,7 +52,7 @@ std::optional getPointIndexAfter( // search forward if (sum_length < offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (offset < sum_length) { return i - 1; } @@ -64,7 +64,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < offset) { return i - 1; } @@ -77,7 +77,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.pose = autoware_universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -152,7 +152,7 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware_universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); @@ -166,7 +166,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware_universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index a48e6c0b836bc..48fc6ae8d4b1f 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_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation @@ -26,7 +27,6 @@ rclcpp_components std_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs visualization_msgs 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 2b8a1bd115856..5e59cd977b655 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -144,7 +144,7 @@ PathSampler::PathSampler(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult PathSampler::onParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "constraints.hard.max_curvature", params_.constraints.hard.max_curvature); updateParam(parameters, "constraints.hard.min_curvature", params_.constraints.hard.min_curvature); @@ -317,14 +317,14 @@ void PathSampler::copyZ( to_traj.front().pose.position.z = from_traj.front().pose.position.z; if (from_traj.size() < 2 || to_traj.size() < 2) return; auto from = from_traj.begin() + 1; - auto s_from = tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_from = autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); auto s_to = 0.0; auto s_from_prev = 0.0; for (auto to = to_traj.begin() + 1; to + 1 != to_traj.end(); ++to) { - s_to += tier4_autoware_utils::calcDistance2d(std::prev(to)->pose, to->pose); + s_to += autoware_universe_utils::calcDistance2d(std::prev(to)->pose, to->pose); for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { s_from_prev = s_from; - s_from += tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + s_from += autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); } const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); to->pose.position.z = std::prev(from)->pose.position.z + @@ -340,8 +340,8 @@ void PathSampler::copyVelocity( if (to_traj.empty() || from_traj.empty()) return; const auto closest_fn = [&](const auto & p1, const auto & p2) { - return tier4_autoware_utils::calcDistance2d(p1.pose, ego_pose) <= - tier4_autoware_utils::calcDistance2d(p2.pose, ego_pose); + return autoware_universe_utils::calcDistance2d(p1.pose, ego_pose) <= + autoware_universe_utils::calcDistance2d(p2.pose, ego_pose); }; const auto first_from = std::min_element(from_traj.begin(), from_traj.end() - 1, closest_fn); const auto first_to = std::min_element(to_traj.begin(), to_traj.end() - 1, closest_fn); @@ -351,14 +351,14 @@ void PathSampler::copyVelocity( to->longitudinal_velocity_mps = first_from->longitudinal_velocity_mps; auto from = first_from; - auto s_from = tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_from = autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); auto s_to = 0.0; auto s_from_prev = 0.0; for (; to + 1 != to_traj.end(); ++to) { - s_to += tier4_autoware_utils::calcDistance2d(to->pose, std::next(to)->pose); + s_to += autoware_universe_utils::calcDistance2d(to->pose, std::next(to)->pose); for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { s_from_prev = s_from; - s_from += tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + s_from += autoware_universe_utils::calcDistance2d(from->pose, std::next(from)->pose); } if ( from->longitudinal_velocity_mps == 0.0 || std::prev(from)->longitudinal_velocity_mps == 0.0) { diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index 64a09087666ff..4d6f2dbd1e008 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -14,11 +14,11 @@ #include "autoware_path_sampler/prepare_inputs.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware_frenet_planner/structures.hpp" #include "autoware_path_sampler/utils/geometry_utils.hpp" #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -40,7 +40,7 @@ void prepareConstraints( constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); for (const auto & o : predicted_objects.objects) if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param - constraints.obstacle_polygons.push_back(tier4_autoware_utils::toPolygon2d(o)); + constraints.obstacle_polygons.push_back(autoware_universe_utils::toPolygon2d(o)); constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented // TODO(Maxime): directly use lines instead of polygon diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index d161d60bc724d..e5d90521eb948 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -47,11 +47,11 @@ void compensateLastPose( const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; const double dist = - tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + autoware_universe_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); const double norm_diff_yaw = [&]() { const double diff_yaw = tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return tier4_autoware_utils::normalizeRadian(diff_yaw); + return autoware_universe_utils::normalizeRadian(diff_yaw); }(); if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { traj_points.push_back(convertToTrajectoryPoint(last_path_point)); diff --git a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index a3ba25b6594e9..de58b02a75623 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ #define AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include @@ -38,13 +38,13 @@ namespace autoware::sampler_common { -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::MultiPolygon2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::MultiPoint2d; +using autoware_universe_utils::MultiPolygon2d; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; -typedef std::pair BoxIndexPair; +typedef std::pair BoxIndexPair; typedef boost::geometry::index::rtree> Rtree; /// @brief data about constraint check results of a given path diff --git a/sensing/image_diagnostics/package.xml b/sensing/image_diagnostics/package.xml index ceb1283b115a5..00d2751b86aa1 100644 --- a/sensing/image_diagnostics/package.xml +++ b/sensing/image_diagnostics/package.xml @@ -12,13 +12,13 @@ ament_cmake_auto + autoware_universe_utils cv_bridge diagnostic_updater image_transport rclcpp rclcpp_components sensor_msgs - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 3dff79bbf9f7f..cbc28e2ff0eeb 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils diagnostic_updater rclcpp rclcpp_components @@ -20,7 +21,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils ament_cmake_gmock ament_cmake_gtest diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp index 9b5719de69524..cdad454452f5c 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -14,7 +14,7 @@ #include "gyro_bias_estimation_module.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -52,15 +52,15 @@ 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; - error_rpy.x = tier4_autoware_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); - error_rpy.y = tier4_autoware_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); - error_rpy.z = tier4_autoware_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); + error_rpy.x = autoware_universe_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); + error_rpy.y = autoware_universe_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); + error_rpy.z = autoware_universe_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); return error_rpy; } diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index d79626f66801b..6ec5c9f64a623 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -14,7 +14,7 @@ #include "gyro_bias_estimator.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -59,7 +59,7 @@ GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) this->get_node_base_interface()->get_context()); this->get_node_timers_interface()->add_timer(timer_, nullptr); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // initialize diagnostics_info_ { @@ -150,10 +150,10 @@ void GyroBiasEstimator::timer_callback() // Check if the vehicle is moving straight const geometry_msgs::msg::Vector3 rpy_0 = - tier4_autoware_utils::getRPY(pose_buf.front().pose.orientation); + autoware_universe_utils::getRPY(pose_buf.front().pose.orientation); const geometry_msgs::msg::Vector3 rpy_1 = - tier4_autoware_utils::getRPY(pose_buf.back().pose.orientation); - const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(rpy_1.z - rpy_0.z)); + autoware_universe_utils::getRPY(pose_buf.back().pose.orientation); + const double yaw_diff = std::abs(autoware_universe_utils::normalizeRadian(rpy_1.z - rpy_0.z)); const double time_diff = (t1_rclcpp_time - t0_rclcpp_time).seconds(); const double yaw_vel = yaw_diff / time_diff; const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 671c18de78f86..7f8d5e9084ce6 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -14,8 +14,8 @@ #ifndef GYRO_BIAS_ESTIMATOR_HPP_ #define GYRO_BIAS_ESTIMATOR_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "gyro_bias_estimation_module.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include #include @@ -76,7 +76,7 @@ class GyroBiasEstimator : public rclcpp::Node std::optional gyro_bias_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string imu_frame_; diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index c8e7698cb4e37..cc4efe1ba2f22 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -25,7 +25,7 @@ std::array transformCovariance(const std::array & cov) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = 0.0; max_cov = std::max(max_cov, cov[COV_IDX::X_X]); @@ -57,7 +57,7 @@ ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) : rclcpp::Node("imu_corrector", options), output_frame_(declare_parameter("base_link", "base_link")) { - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); angular_velocity_offset_x_imu_link_ = declare_parameter("angular_velocity_offset_x", 0.0); angular_velocity_offset_y_imu_link_ = declare_parameter("angular_velocity_offset_y", 0.0); diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp index 4d5377274ae78..45f7b4e6142b2 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -14,8 +14,8 @@ #ifndef IMU_CORRECTOR_CORE_HPP_ #define IMU_CORRECTOR_CORE_HPP_ -#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" #include @@ -31,7 +31,7 @@ namespace imu_corrector { class ImuCorrector : public rclcpp::Node { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; public: explicit ImuCorrector(const rclcpp::NodeOptions & options); @@ -53,7 +53,7 @@ class ImuCorrector : public rclcpp::Node double accel_stddev_imu_link_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string output_frame_; }; diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index ecb84c8a7f3b3..df62f538e9964 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -37,7 +37,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base rclcpp sensor_msgs tf2_ros - tier4_autoware_utils + autoware_universe_utils pcl_ros ) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 84b056bec37b7..22fbbd09bd8fc 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -63,10 +63,10 @@ // ROS includes #include "autoware_point_types/types.hpp" +#include +#include #include #include -#include -#include #include #include @@ -185,8 +185,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node const std::string & original_topic_name, const std::string & postfix); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index 4e183a9816c2e..bd67759d3d7a6 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -63,10 +63,10 @@ // ROS includes #include "autoware_point_types/types.hpp" +#include +#include #include #include -#include -#include #include #include @@ -172,8 +172,8 @@ class PointCloudConcatenationComponent : public rclcpp::Node void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 618957a2ac783..4b018f22aa60a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -36,8 +36,8 @@ #include // Include tier4 autoware utils -#include -#include +#include +#include #include #include @@ -69,8 +69,8 @@ class DistortionCorrectorComponent : public rclcpp::Node rclcpp::Subscription::SharedPtr twist_sub_; rclcpp::Publisher::SharedPtr undistorted_points_pub_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; tf2_ros::Buffer tf2_buffer_{get_clock()}; tf2_ros::TransformListener tf2_listener_{tf2_buffer_}; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index 1f23c975a88af..23deceaf29c21 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -80,9 +80,9 @@ #include // Include tier4 autoware utils -#include -#include -#include +#include +#include +#include namespace pointcloud_preprocessor { @@ -177,9 +177,9 @@ class Filter : public rclcpp::Node std::mutex mutex_; /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 9330b1e998b5b..c89c97269fd2e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -63,10 +63,10 @@ // ROS includes #include "autoware_point_types/types.hpp" +#include +#include #include #include -#include -#include #include #include @@ -182,8 +182,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node const std::string & original_topic_name, const std::string & postfix); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index 42078922d39b9..b3ab34da2726f 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -15,10 +15,10 @@ #ifndef POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ #define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#include #include #include #include -#include #include #include @@ -40,9 +40,9 @@ #include #include -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::Point2d; +using autoware_universe_utils::LinearRing2d; +using autoware_universe_utils::MultiPoint2d; +using autoware_universe_utils::Point2d; namespace pointcloud_preprocessor { diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 760ffbe1eb1dd..cd34c63b50de7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -18,15 +18,15 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/utility/utilities.hpp" +#include #include #include -#include #include #include -using tier4_autoware_utils::MultiPoint2d; +using autoware_universe_utils::MultiPoint2d; namespace pointcloud_preprocessor { diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 619bf3180591d..a669e94ee8aae 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -25,6 +25,7 @@ autoware_cmake autoware_point_types + autoware_universe_utils cgal cv_bridge diagnostic_updater @@ -46,7 +47,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_pcl_extensions diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 402ceb65f6594..bd96ef9d215bd 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -75,8 +75,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 46c9e681c3de2..d442a91a91c5a 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -37,8 +37,8 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "concatenate_pointclouds_debug"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index cfbeffee9982c..79632b7a4111c 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -62,8 +62,8 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index b90a64dc47c91..7b9ed5f30a3d8 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,7 +14,7 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" +#include "autoware/universe_utils/math/trigonometry.hpp" #include #include @@ -28,8 +28,8 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distortion_corrector"); stop_watch_ptr_->tic("cyclic_time"); @@ -290,11 +290,11 @@ bool DistortionCorrectorComponent::undistortPointCloud( theta += w * time_offset; baselink_quat.setValue( - 0, 0, tier4_autoware_utils::sin(theta * 0.5f), - tier4_autoware_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + 0, 0, autoware_universe_utils::sin(theta * 0.5f), + autoware_universe_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); const float dis = v * time_offset; - x += dis * tier4_autoware_utils::cos(theta); - y += dis * tier4_autoware_utils::sin(theta); + x += dis * autoware_universe_utils::cos(theta); + y += dis * autoware_universe_utils::sin(theta); baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); baselink_tf_odom.setRotation(baselink_quat); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp index 6393a935562ac..2114810b8934c 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp @@ -57,8 +57,8 @@ PickupBasedVoxelGridDownsampleFilterComponent::PickupBasedVoxelGridDownsampleFil { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index 5584fd65c9acb..d289aa9c95ea1 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -102,7 +102,8 @@ pointcloud_preprocessor::Filter::Filter( set_param_res_filter_ = this->add_on_set_parameters_callback( std::bind(&Filter::filterParamCallback, this, std::placeholders::_1)); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); } diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index ada7d2616ed38..975c04c9089d6 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -29,8 +29,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); outlier_pointcloud_publisher_ = diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 12b034043894e..42a1ef68d0f68 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -46,8 +46,8 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware_universe_utils::DebugPublisher; + using autoware_universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "time_synchronizer"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 6b4e90a697cd6..d35aca8decc28 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -16,7 +16,7 @@ namespace { -tier4_autoware_utils::Box2d calcBoundingBox( +autoware_universe_utils::Box2d calcBoundingBox( const pcl::PointCloud::ConstPtr & input_cloud) { MultiPoint2d candidate_points; @@ -24,11 +24,11 @@ tier4_autoware_utils::Box2d calcBoundingBox( candidate_points.emplace_back(p.x, p.y); } - return boost::geometry::return_envelope(candidate_points); + return boost::geometry::return_envelope(candidate_points); } lanelet::ConstPolygons3d calcIntersectedPolygons( - const tier4_autoware_utils::Box2d & bounding_box, const lanelet::ConstPolygons3d & polygons) + const autoware_universe_utils::Box2d & bounding_box, const lanelet::ConstPolygons3d & polygons) { lanelet::ConstPolygons3d intersected_polygons; for (const auto & polygon : polygons) { diff --git a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp index f1ab9344d58f3..ee9acbddf93b1 100644 --- a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp +++ b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp @@ -15,7 +15,7 @@ #ifndef RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ #define RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ -#include "tier4_autoware_utils/ros/transform_listener.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" #include @@ -51,7 +51,7 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node // Subscriber message_filters::Subscriber sub_radar_{}; message_filters::Subscriber sub_odometry_{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; using SyncPolicy = message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index 05453ee9cb9e0..b1063562b1708 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils message_filters nav_msgs radar_msgs @@ -22,7 +23,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils ament_clang_format ament_lint_auto diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp index 0eda1eae627e7..e148ed275af30 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -96,7 +96,7 @@ RadarStaticPointcloudFilterNode::RadarStaticPointcloudFilterNode( node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd"); // Subscriber - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_radar_.subscribe(this, "~/input/radar", rclcpp::QoS{1}.get_rmw_qos_profile()); sub_odometry_.subscribe(this, "~/input/odometry", rclcpp::QoS{1}.get_rmw_qos_profile()); diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index 4704024214d9e..caeb01c6823e5 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -19,6 +19,7 @@ autoware_lint_common autoware_perception_msgs + autoware_universe_utils geometry_msgs libpcl-all-dev pcl_conversions @@ -28,7 +29,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 9c58961978161..184bef8c96820 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -14,7 +14,7 @@ #include "dummy_perception_publisher/node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -93,7 +93,7 @@ ObjectInfo::ObjectInfo( } const auto current_pose = - tier4_autoware_utils::calcOffsetPose(initial_pose, move_distance, 0.0, 0.0); + autoware_universe_utils::calcOffsetPose(initial_pose, move_distance, 0.0, 0.0); // calculate tf from map to moved_object geometry_msgs::msg::Transform ros_map2moved_object; diff --git a/simulator/fault_injection/package.xml b/simulator/fault_injection/package.xml index e169a338b4e52..57d1fa3401f41 100644 --- a/simulator/fault_injection/package.xml +++ b/simulator/fault_injection/package.xml @@ -10,13 +10,13 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils diagnostic_aggregator diagnostic_msgs diagnostic_updater pluginlib rclcpp rclcpp_components - tier4_autoware_utils tier4_simulation_msgs ament_cmake_ros diff --git a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp index 7b8f87400beab..467054dcda8fe 100644 --- a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp +++ b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp @@ -14,7 +14,7 @@ #include "fault_injection/fault_injection_node.hpp" -#include +#include #include #include diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index b820c5c6c3df0..95ac530eaf423 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_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs @@ -33,7 +34,6 @@ tf2_geometry_msgs tf2_ros tier4_api_utils - tier4_autoware_utils tier4_external_api_msgs tier4_vehicle_msgs autoware_cmake 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 c66a0c6f1f922..bcad2d03cac4b 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/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/ros/update_param.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 #include @@ -59,7 +59,7 @@ nav_msgs::msg::Odometry to_odometry( nav_msgs::msg::Odometry odometry; odometry.pose.pose.position.x = vehicle_model_ptr->getX(); odometry.pose.pose.position.y = vehicle_model_ptr->getY(); - odometry.pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY( + odometry.pose.pose.orientation = autoware_universe_utils::createQuaternionFromRPY( 0.0, ego_pitch_angle, vehicle_model_ptr->getYaw()); odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); @@ -304,8 +304,8 @@ rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( result.reason = "success"; try { - tier4_autoware_utils::updateParam(parameters, "x_stddev", x_stddev_); - tier4_autoware_utils::updateParam(parameters, "y_stddev", y_stddev_); + autoware_universe_utils::updateParam(parameters, "x_stddev", x_stddev_); + autoware_universe_utils::updateParam(parameters, "y_stddev", y_stddev_); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -323,7 +323,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const geometry_msgs::msg::Pose ego_pose; ego_pose.position.x = ego_x; ego_pose.position.y = ego_y; - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.orientation = autoware_universe_utils::createQuaternionFromYaw(ego_yaw); // calculate prev/next point of lanelet centerline nearest to ego pose. lanelet::Lanelet ego_lanelet; @@ -398,7 +398,7 @@ void SimplePlanningSimulator::on_timer() // add estimate covariance { - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; current_odometry_.pose.covariance[COV_IDX::X_X] = x_stddev_; current_odometry_.pose.covariance[COV_IDX::Y_Y] = y_stddev_; } @@ -553,7 +553,7 @@ void SimplePlanningSimulator::add_measurement_noise( odom.twist.twist.linear.x += velocity_noise; double yaw = tf2::getYaw(odom.pose.pose.orientation); yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); - odom.pose.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + odom.pose.pose.orientation = autoware_universe_utils::createQuaternionFromYaw(yaw); vel.longitudinal_velocity += static_cast(velocity_noise); @@ -686,7 +686,7 @@ void SimplePlanningSimulator::publish_acceleration() msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); msg.accel.accel.linear.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; msg.accel.covariance.at(COV_IDX::X_X) = COV; // linear x msg.accel.covariance.at(COV_IDX::Y_Y) = COV; // linear y @@ -699,7 +699,7 @@ void SimplePlanningSimulator::publish_acceleration() void SimplePlanningSimulator::publish_imu() { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_universe_utils::xyz_covariance_index::XYZ_COV_IDX; sensor_msgs::msg::Imu imu; imu.header.frame_id = "base_link"; diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml index 64441ad338f2f..befe02ecb330e 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/dummy_diag_publisher/package.xml @@ -11,11 +11,11 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils diagnostic_updater fmt rclcpp rclcpp_components - tier4_autoware_utils rqt_reconfigure rqt_robot_monitor diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index 9abf325e62833..c20bcda5a6b70 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -17,10 +17,10 @@ #include #define FMT_HEADER_ONLY +#include #include #include #include -#include #include @@ -66,8 +66,8 @@ rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::paramCallback( auto prev_status_str = status_str; auto is_active = true; try { - tier4_autoware_utils::updateParam(parameters, status_prefix_str, status_str); - tier4_autoware_utils::updateParam(parameters, is_active_prefix_str, is_active); + autoware_universe_utils::updateParam(parameters, status_prefix_str, status_str); + autoware_universe_utils::updateParam(parameters, is_active_prefix_str, is_active); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index cd66cde64c9a5..798da2374b62a 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -20,7 +20,7 @@ #include // Autoware -#include +#include #include #include @@ -63,13 +63,14 @@ class EmergencyHandler : public rclcpp::Node sub_hazard_status_stamped_; rclcpp::Subscription::SharedPtr sub_prev_control_command_; // Subscribers without callback - tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber< + autoware_vehicle_msgs::msg::ControlModeReport> sub_control_mode_{this, "~/input/control_mode"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index f26080fd8ef56..099e8a77d191e 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -15,13 +15,13 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components std_msgs std_srvs - tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml index 74ee8c7183741..44c80485d5b1b 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/mrm_emergency_stop_operator/package.xml @@ -13,11 +13,11 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_universe_utils rclcpp rclcpp_components std_msgs std_srvs - tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 589a2fd14d990..e6bac26677be5 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -14,7 +14,7 @@ #include "mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp" -#include +#include namespace mrm_emergency_stop_operator { @@ -59,7 +59,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n rcl_interfaces::msg::SetParametersResult MrmEmergencyStopOperator::onParameter( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware_universe_utils::updateParam; updateParam(parameters, "target_acceleration", params_.target_acceleration); updateParam(parameters, "target_jerk", params_.target_jerk); diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 33d8af5004c47..feb01bfa22d3f 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -22,7 +22,7 @@ #include // Autoware -#include +#include #include #include @@ -72,17 +72,18 @@ class MrmHandler : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; // Subscribers without callback - tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber< + autoware_vehicle_msgs::msg::ControlModeReport> sub_control_mode_{this, "~/input/control_mode"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_mrm_pull_over_status_{this, "~/input/mrm/pull_over/status"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware_universe_utils::InterProcessPollingSubscriber sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_universe_utils::InterProcessPollingSubscriber< autoware_adapi_v1_msgs::msg::OperationModeState> sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index 5774cce3215a9..a2c3397296db3 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -15,13 +15,13 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components std_msgs std_srvs - tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 22f1f4e9012c7..8795d12718325 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -15,7 +15,7 @@ #ifndef SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ #define SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" #include #include @@ -147,7 +147,7 @@ class AutowareErrorMonitor : public rclcpp::Node void loggingErrors(const autoware_system_msgs::msg::HazardStatus & diag_array); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; #endif // SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml index 0c8e2fc0241aa..2dda4e462b98b 100644 --- a/system/system_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -11,13 +11,13 @@ autoware_cmake autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs diagnostic_msgs fmt rclcpp rclcpp_components std_srvs - tier4_autoware_utils tier4_control_msgs diagnostic_aggregator diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index ad67521298643..d00426137221f 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -265,7 +265,7 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&AutowareErrorMonitor::onTimer, this)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void AutowareErrorMonitor::loadRequiredModules(const std::string & key) diff --git a/system/system_monitor/package.xml b/system/system_monitor/package.xml index bbb01b5437fbe..880c885721fbc 100644 --- a/system/system_monitor/package.xml +++ b/system/system_monitor/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils diagnostic_msgs diagnostic_updater fmt @@ -22,7 +23,6 @@ rclcpp rclcpp_components std_msgs - tier4_autoware_utils tier4_external_api_msgs chrony diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 0b91b28b2b5f9..4cacfb5f9770f 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -19,7 +19,7 @@ #include "system_monitor/ntp_monitor/ntp_monitor.hpp" -#include +#include #include #include @@ -126,7 +126,7 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) void NTPMonitor::onTimer() { // Start to measure elapsed time - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; stop_watch.tic("execution_time"); std::string error_str; diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 1173431b0f7c2..55636c7893d5d 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -21,7 +21,7 @@ #include "system_monitor/system_monitor_utility.hpp" -#include +#include #include @@ -517,7 +517,7 @@ void ProcessMonitor::onTimer() bool is_top_error = false; // Start to measure elapsed time - tier4_autoware_utils::StopWatch stop_watch; + autoware_universe_utils::StopWatch stop_watch; stop_watch.tic("execution_time"); int out_fd[2]; diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp index 4a31da363deff..17e3786eff7fe 100644 --- a/tools/reaction_analyzer/include/utils.hpp +++ b/tools/reaction_analyzer/include/utils.hpp @@ -15,12 +15,12 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ +#include +#include #include #include #include #include -#include -#include #include #include diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml index a5199aa78c9f8..7d5c8b61374d7 100644 --- a/tools/reaction_analyzer/package.xml +++ b/tools/reaction_analyzer/package.xml @@ -20,6 +20,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs eigen libpcl-all-dev @@ -35,7 +36,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils ament_cmake_ros ament_index_python diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 53313f28eb4d9..79e64ba514bfe 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -205,7 +205,7 @@ void ReactionAnalyzerNode::spawn_obstacle(const geometry_msgs::msg::Point & ego_ } } else { if ( - tier4_autoware_utils::calcDistance3d(ego_pose, entity_pose_.position) < + autoware_universe_utils::calcDistance3d(ego_pose, entity_pose_.position) < node_params_.spawn_distance_threshold) { if (!spawn_object_cmd_) { spawn_object_cmd_ = true; @@ -394,8 +394,8 @@ bool ReactionAnalyzerNode::check_ego_init_correctly( } constexpr double deviation_threshold = 0.1; - const auto deviation = - tier4_autoware_utils::calcPoseDeviation(ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); + const auto deviation = autoware_universe_utils::calcPoseDeviation( + ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); const bool is_position_initialized_correctly = deviation.longitudinal < deviation_threshold && deviation.lateral < deviation_threshold && deviation.yaw < deviation_threshold; diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index 9f9198af7dc48..a07743417fa80 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -136,8 +136,8 @@ visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & pa tf2::Quaternion quaternion; quaternion.setRPY( - tier4_autoware_utils::deg2rad(params.roll), tier4_autoware_utils::deg2rad(params.pitch), - tier4_autoware_utils::deg2rad(params.yaw)); + autoware_universe_utils::deg2rad(params.roll), autoware_universe_utils::deg2rad(params.pitch), + autoware_universe_utils::deg2rad(params.yaw)); marker.pose.orientation = tf2::toMsg(quaternion); marker.scale.x = 0.1; // Line width @@ -224,7 +224,7 @@ size_t get_index_after_distance( size_t target_id = curr_id; double current_distance = 0.0; for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { - current_distance = tier4_autoware_utils::calcDistance3d(traj.points.at(traj_id), curr_p); + current_distance = autoware_universe_utils::calcDistance3d(traj.points.at(traj_id), curr_p); if (current_distance >= distance) { break; } @@ -286,9 +286,9 @@ geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params) tf2::Quaternion entity_q_orientation; entity_q_orientation.setRPY( - tier4_autoware_utils::deg2rad(entity_params.roll), - tier4_autoware_utils::deg2rad(entity_params.pitch), - tier4_autoware_utils::deg2rad(entity_params.yaw)); + autoware_universe_utils::deg2rad(entity_params.roll), + autoware_universe_utils::deg2rad(entity_params.pitch), + autoware_universe_utils::deg2rad(entity_params.yaw)); entity_pose.orientation = tf2::toMsg(entity_q_orientation); return entity_pose; } @@ -302,9 +302,9 @@ geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params) tf2::Quaternion pose_q_orientation; pose_q_orientation.setRPY( - tier4_autoware_utils::deg2rad(pose_params.roll), - tier4_autoware_utils::deg2rad(pose_params.pitch), - tier4_autoware_utils::deg2rad(pose_params.yaw)); + autoware_universe_utils::deg2rad(pose_params.roll), + autoware_universe_utils::deg2rad(pose_params.pitch), + autoware_universe_utils::deg2rad(pose_params.yaw)); pose.orientation = tf2::toMsg(pose_q_orientation); return pose; } @@ -316,9 +316,9 @@ PointCloud2::SharedPtr create_entity_pointcloud_ptr( tf2::Quaternion entity_q_orientation; entity_q_orientation.setRPY( - tier4_autoware_utils::deg2rad(entity_params.roll), - tier4_autoware_utils::deg2rad(entity_params.pitch), - tier4_autoware_utils::deg2rad(entity_params.yaw)); + autoware_universe_utils::deg2rad(entity_params.roll), + autoware_universe_utils::deg2rad(entity_params.pitch), + autoware_universe_utils::deg2rad(entity_params.yaw)); tf2::Transform tf(entity_q_orientation); const auto origin = tf2::Vector3(entity_params.x, entity_params.y, entity_params.z); tf.setOrigin(origin); @@ -395,7 +395,7 @@ bool search_pointcloud_near_pose( return std::any_of( pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), [pose, search_radius](const auto & point) { - return tier4_autoware_utils::calcDistance3d(pose.position, point) <= search_radius; + return autoware_universe_utils::calcDistance3d(pose.position, point) <= search_radius; }); } @@ -406,7 +406,7 @@ bool search_predicted_objects_near_pose( return std::any_of( predicted_objects.objects.begin(), predicted_objects.objects.end(), [pose, search_radius](const PredictedObject & object) { - return tier4_autoware_utils::calcDistance3d( + return autoware_universe_utils::calcDistance3d( pose.position, object.kinematics.initial_pose_with_covariance.pose.position) <= search_radius; }); @@ -420,7 +420,7 @@ bool search_detected_objects_near_pose( return std::any_of( detected_objects.objects.begin(), detected_objects.objects.end(), [pose, search_radius](const DetectedObject & object) { - return tier4_autoware_utils::calcDistance3d( + return autoware_universe_utils::calcDistance3d( pose.position, object.kinematics.pose_with_covariance.pose.position) <= search_radius; }); @@ -433,7 +433,7 @@ bool search_tracked_objects_near_pose( return std::any_of( tracked_objects.objects.begin(), tracked_objects.objects.end(), [pose, search_radius](const TrackedObject & object) { - return tier4_autoware_utils::calcDistance3d( + return autoware_universe_utils::calcDistance3d( pose.position, object.kinematics.pose_with_covariance.pose.position) <= search_radius; }); diff --git a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 7d32fdc357510..6446511db4c29 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -17,14 +17,14 @@ #ifndef AUTOWARE_ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ #define AUTOWARE_ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "autoware_raw_vehicle_cmd_converter/accel_map.hpp" #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -88,7 +88,7 @@ using DataStampedPtr = std::shared_ptr; class AccelBrakeMapCalibrator : public rclcpp::Node { private: - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string csv_default_map_dir_; rclcpp::Publisher::SharedPtr original_map_occ_pub_; rclcpp::Publisher::SharedPtr update_map_occ_pub_; @@ -107,13 +107,13 @@ class AccelBrakeMapCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr map_error_ratio_pub_; rclcpp::Publisher::SharedPtr calibration_status_pub_; - tier4_autoware_utils::InterProcessPollingSubscriber steer_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber steer_sub_{ this, "~/input/steer"}; - tier4_autoware_utils::InterProcessPollingSubscriber actuation_status_sub_{ - this, "~/input/actuation_status"}; - tier4_autoware_utils::InterProcessPollingSubscriber actuation_cmd_sub_{ - this, "~/input/actuation_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber velocity_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber + actuation_status_sub_{this, "~/input/actuation_status"}; + autoware_universe_utils::InterProcessPollingSubscriber + actuation_cmd_sub_{this, "~/input/actuation_cmd"}; + autoware_universe_utils::InterProcessPollingSubscriber velocity_sub_{ this, "~/input/velocity"}; // Service @@ -374,7 +374,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node COMMAND = 1, }; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; public: explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); diff --git a/vehicle/autoware_accel_brake_map_calibrator/package.xml b/vehicle/autoware_accel_brake_map_calibrator/package.xml index 335ec2bba2ac3..97e79f921a41e 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/package.xml +++ b/vehicle/autoware_accel_brake_map_calibrator/package.xml @@ -15,6 +15,7 @@ autoware_cmake autoware_raw_vehicle_cmd_converter + autoware_universe_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs @@ -24,7 +25,6 @@ std_srvs tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_external_api_msgs tier4_vehicle_msgs diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index ee0b7531d46cb..15d7473d9f00a 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -32,7 +32,7 @@ namespace autoware::accel_brake_map_calibrator AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options) : Node("accel_brake_map_calibrator", node_options) { - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // get parameter update_hz_ = declare_parameter("update_hz", 10.0); covariance_ = declare_parameter("initial_covariance", 0.05); @@ -217,7 +217,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod init_timer(1.0 / update_hz_); init_output_csv_timer(30.0); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void AccelBrakeMapCalibrator::init_output_csv_timer(double period_s) diff --git a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp index 97ee325ecdcca..2f1c0ac74b9a5 100644 --- a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp +++ b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ #define AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include #include @@ -60,10 +60,11 @@ class ExternalCmdConverterNode : public rclcpp::Node emergency_stop_heartbeat_sub_; // Polling Subscriber - tier4_autoware_utils::InterProcessPollingSubscriber velocity_sub_{this, "in/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber shift_cmd_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber velocity_sub_{ + this, "in/odometry"}; + autoware_universe_utils::InterProcessPollingSubscriber shift_cmd_sub_{ this, "in/shift_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber gate_mode_sub_{ + autoware_universe_utils::InterProcessPollingSubscriber gate_mode_sub_{ this, "in/current_gate_mode"}; void on_external_cmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr); diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp index 26b3dde029ebc..b807e74fc27cf 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_ #define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware_raw_vehicle_cmd_converter/accel_map.hpp" #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" #include "autoware_raw_vehicle_cmd_converter/pid.hpp" #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include @@ -78,9 +78,9 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief subscriber for vehicle command rclcpp::Subscription::SharedPtr sub_control_cmd_; // polling subscribers - tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_steering_{ + autoware_universe_utils::InterProcessPollingSubscriber sub_steering_{ this, "~/input/steering"}; rclcpp::TimerBase::SharedPtr timer_; @@ -117,7 +117,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; DebugValues debug_steer_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index 0051dea0a5103..ce783cfcd32e4 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -81,7 +81,7 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml index 08db4f5903abe..5e4638f605713 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_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater @@ -16,7 +17,6 @@ rclcpp rclcpp_components std_msgs - tier4_autoware_utils tier4_debug_msgs global_parameter_loader pose2twist diff --git a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp index c94ea01224da2..0fcc5fcf28863 100644 --- a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ #define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" namespace autoware::vehicle_info_utils { @@ -47,8 +47,8 @@ struct VehicleInfo double min_height_offset_m; double max_height_offset_m; - tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const; - tier4_autoware_utils::LinearRing2d createFootprint( + autoware_universe_utils::LinearRing2d createFootprint(const double margin = 0.0) const; + autoware_universe_utils::LinearRing2d createFootprint( const double lat_margin, const double lon_margin) const; }; diff --git a/vehicle/autoware_vehicle_info_utils/package.xml b/vehicle/autoware_vehicle_info_utils/package.xml index bd8ec0cb601a0..c5d09bd039691 100644 --- a/vehicle/autoware_vehicle_info_utils/package.xml +++ b/vehicle/autoware_vehicle_info_utils/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils rclcpp - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp index c7f587d20b2f4..665ee61fa67ff 100644 --- a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -16,16 +16,16 @@ namespace autoware::vehicle_info_utils { -tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const +autoware_universe_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const { return createFootprint(margin, margin); } -tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint( +autoware_universe_utils::LinearRing2d VehicleInfo::createFootprint( const double lat_margin, const double lon_margin) const { - using tier4_autoware_utils::LinearRing2d; - using tier4_autoware_utils::Point2d; + using autoware_universe_utils::LinearRing2d; + using autoware_universe_utils::Point2d; const double x_front = front_overhang_m + wheel_base_m + lon_margin; const double x_center = wheel_base_m / 2.0;