diff --git a/common/autoware_utils/CMakeLists.txt b/common/autoware_utils/CMakeLists.txt new file mode 100644 index 0000000000000..d6c7c1308de2d --- /dev/null +++ b/common/autoware_utils/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_utils) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Boost REQUIRED) + +ament_auto_add_library(autoware_utils SHARED + src/autoware_utils.cpp + src/planning/planning_marker_helper.cpp +) + +# Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + file(GLOB_RECURSE test_files test/**/*.cpp) + + ament_add_gtest(test_autoware_utils ${test_files}) + + target_link_libraries(test_autoware_utils + autoware_utils + ) +endif() + +ament_auto_package() diff --git a/common/autoware_utils/README.md b/common/autoware_utils/README.md new file mode 100644 index 0000000000000..42ca7e498f87a --- /dev/null +++ b/common/autoware_utils/README.md @@ -0,0 +1,5 @@ +# autoware_utils + +## Purpose + +This package contains many common functions used by other packages, so please refer to them as needed. diff --git a/common/autoware_utils/include/autoware_utils/autoware_utils.hpp b/common/autoware_utils/include/autoware_utils/autoware_utils.hpp new file mode 100644 index 0000000000000..19542dd07a8fd --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/autoware_utils.hpp @@ -0,0 +1,37 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_ +#define AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_ + +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/math/constants.hpp" +#include "autoware_utils/math/normalization.hpp" +#include "autoware_utils/math/range.hpp" +#include "autoware_utils/math/unit_conversion.hpp" +#include "autoware_utils/planning/planning_marker_helper.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/debug_traits.hpp" +#include "autoware_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/processing_time_publisher.hpp" +#include "autoware_utils/ros/self_pose_listener.hpp" +#include "autoware_utils/ros/transform_listener.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/ros/wait_for_param.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" + +#endif // AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp b/common/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp new file mode 100644 index 0000000000000..743bafceb0ace --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp @@ -0,0 +1,96 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#define AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +#include + +namespace autoware_utils +{ +// 2D +struct Point2d; +using Segment2d = boost::geometry::model::segment; +using Box2d = boost::geometry::model::box; +using LineString2d = boost::geometry::model::linestring; +using LinearRing2d = boost::geometry::model::ring; +using Polygon2d = boost::geometry::model::polygon; +using MultiPoint2d = boost::geometry::model::multi_point; +using MultiLineString2d = boost::geometry::model::multi_linestring; +using MultiPolygon2d = boost::geometry::model::multi_polygon; + +// 3D +struct Point3d; +using Segment3d = boost::geometry::model::segment; +using Box3d = boost::geometry::model::box; +using LineString3d = boost::geometry::model::linestring; +using LinearRing3d = boost::geometry::model::ring; +using Polygon3d = boost::geometry::model::polygon; +using MultiPoint3d = boost::geometry::model::multi_point; +using MultiLineString3d = boost::geometry::model::multi_linestring; +using MultiPolygon3d = boost::geometry::model::multi_polygon; + +struct Point2d : public Eigen::Vector2d +{ + Point2d() = default; + Point2d(const double x, const double y) : Eigen::Vector2d(x, y) {} + + [[nodiscard]] Point3d to_3d(const double z = 0.0) const; +}; + +struct Point3d : public Eigen::Vector3d +{ + Point3d() = default; + Point3d(const double x, const double y, const double z) : Eigen::Vector3d(x, y, z) {} + + [[nodiscard]] Point2d to_2d() const; +}; + +inline Point3d Point2d::to_3d(const double z) const { return Point3d{x(), y(), z}; } + +inline Point2d Point3d::to_2d() const { return Point2d{x(), y()}; } + +inline geometry_msgs::msg::Point toMsg(const Point3d & point) +{ + geometry_msgs::msg::Point msg; + msg.x = point.x(); + msg.y = point.y(); + msg.z = point.z(); + return msg; +} + +inline Point3d fromMsg(const geometry_msgs::msg::Point & msg) +{ + Point3d point; + point.x() = msg.x; + point.y() = msg.y; + point.z() = msg.z; + return point; +} +} // namespace autoware_utils + +BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT + autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT + autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT + +#endif // AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/geometry/geometry.hpp b/common/autoware_utils/include/autoware_utils/geometry/geometry.hpp new file mode 100644 index 0000000000000..aa03ffabef3fd --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/geometry/geometry.hpp @@ -0,0 +1,357 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#define AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "autoware_utils/geometry/boost_geometry.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace autoware_utils +{ +template +geometry_msgs::msg::Point getPoint(const T & p) +{ + return geometry_msgs::build().x(p.x).y(p.y).z(p.z); +} + +template <> +inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) +{ + return p; +} + +template <> +inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Pose & p) +{ + return p.position; +} + +template <> +inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseStamped & p) +{ + return p.pose.position; +} + +template <> +inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseWithCovarianceStamped & p) +{ + return p.pose.pose.position; +} + +template <> +inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p) +{ + return p.pose.position; +} + +template <> +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +{ + return p.pose.position; +} + +template <> +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose.position; +} + +template +geometry_msgs::msg::Pose getPose([[maybe_unused]] const T & p) +{ + static_assert(sizeof(T) == 0, "Only specializations of getPose can be used."); + throw std::logic_error("Only specializations of getPose can be used."); +} + +template <> +inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::Pose & p) +{ + return p; +} + +template <> +inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::PoseStamped & p) +{ + return p.pose; +} + +template <> +inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::PathPoint & p) +{ + return p.pose; +} + +template <> +inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +{ + return p.pose; +} + +inline geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Quaternion & quat) +{ + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +{ + return getRPY(pose.orientation); +} + +inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose) +{ + return getRPY(pose.pose); +} + +inline geometry_msgs::msg::Vector3 getRPY( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + return getRPY(pose.pose.pose); +} + +inline geometry_msgs::msg::Quaternion createQuaternion( + const double x, const double y, const double z, const double w) +{ + geometry_msgs::msg::Quaternion q; + q.x = x; + q.y = y; + q.z = z; + q.w = w; + return q; +} + +inline geometry_msgs::msg::Vector3 createTranslation(const double x, const double y, const double z) +{ + geometry_msgs::msg::Vector3 v; + v.x = x; + v.y = y; + v.z = z; + return v; +} + +// Revival of tf::createQuaternionFromRPY +// https://answers.ros.org/question/304397/recommended-way-to-construct-quaternion-from-rollpitchyaw-with-tf2/ +inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( + const double roll, const double pitch, const double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} + +inline geometry_msgs::msg::Quaternion createQuaternionFromYaw(const double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); +} + +template +double calcDistance2d(const Point1 & point1, const Point2 & point2) +{ + const auto p1 = getPoint(point1); + const auto p2 = getPoint(point2); + return std::hypot(p1.x - p2.x, p1.y - p2.y); +} + +template +double calcSquaredDistance2d(const Point1 & point1, const Point2 & point2) +{ + const auto p1 = getPoint(point1); + const auto p2 = getPoint(point2); + const auto dx = p1.x - p2.x; + const auto dy = p1.y - p2.y; + return dx * dx + dy * dy; +} + +template +double calcDistance3d(const Point1 & point1, const Point2 & point2) +{ + const auto p1 = getPoint(point1); + const auto p2 = getPoint(point2); + // To be replaced by std::hypot(dx, dy, dz) in C++17 + return std::hypot(std::hypot(p1.x - p2.x, p1.y - p2.y), p1.z - p2.z); +} + +/** + * @brief calculate elevation angle of two points. + * @details This function returns the elevation angle of the position of the two input points + * with respect to the origin of their coordinate system. + * If the two points are in the same position, the calculation result will be unstable. + * @param p_from source point + * @param p_to target point + * @return -pi/2 <= elevation angle <= pi/2 + */ +inline double calcElevationAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) +{ + const double dz = p_to.z - p_from.z; + const double dist_2d = calcDistance2d(p_from, p_to); + return std::atan2(dz, dist_2d); +} + +/** + * @brief calculate azimuth angle of two points. + * @details This function returns the azimuth angle of the position of the two input points + * with respect to the origin of their coordinate system. + * If x and y of the two points are the same, the calculation result will be unstable. + * @param p_from source point + * @param p_to target point + * @return -pi < azimuth angle < pi. + */ +inline double calcAzimuthAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) +{ + const double dx = p_to.x - p_from.x; + const double dy = p_to.y - p_from.y; + return std::atan2(dy, dx); +} + +inline geometry_msgs::msg::Pose transform2pose(const geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = transform.translation.x; + pose.position.y = transform.translation.y; + pose.position.z = transform.translation.z; + pose.orientation = transform.rotation; + return pose; +} + +inline geometry_msgs::msg::PoseStamped transform2pose( + const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header = transform.header; + pose.pose = transform2pose(transform.transform); + return pose; +} + +inline geometry_msgs::msg::Transform pose2transform(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = pose.position.x; + transform.translation.y = pose.position.y; + transform.translation.z = pose.position.z; + transform.rotation = pose.orientation; + return transform; +} + +inline geometry_msgs::msg::TransformStamped pose2transform( + const geometry_msgs::msg::PoseStamped & pose, const std::string child_frame_id) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header = pose.header; + transform.transform = pose2transform(pose.pose); + transform.child_frame_id = child_frame_id; + return transform; +} + +inline Point3d transformPoint( + const Point3d & point, const geometry_msgs::msg::Transform & transform) +{ + const auto & translation = transform.translation; + const auto & rotation = transform.rotation; + + const Eigen::Translation3d T(translation.x, translation.y, translation.z); + const Eigen::Quaterniond R(rotation.w, rotation.x, rotation.y, rotation.z); + + const Eigen::Vector3d transformed(T * R * point); + + return Point3d{transformed.x(), transformed.y(), transformed.z()}; +} + +inline Point2d transformPoint( + const Point2d & point, const geometry_msgs::msg::Transform & transform) +{ + Point3d point_3d{point.x(), point.y(), 0}; + const auto transformed = transformPoint(point_3d, transform); + return Point2d{transformed.x(), transformed.y()}; +} + +template +T transformVector(const T & points, const geometry_msgs::msg::Transform & transform) +{ + T transformed; + for (const auto & point : points) { + transformed.push_back(transformPoint(point, transform)); + } + return transformed; +} + +inline double calcCurvature( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3) +{ + // Calculation details are described in the following page + // https://en.wikipedia.org/wiki/Menger_curvature + const double denominator = + calcDistance2d(p1, p2) * calcDistance2d(p2, p3) * calcDistance2d(p3, p1); + if (std::fabs(denominator) < 1e-10) { + throw std::runtime_error("points are too close for curvature calculation."); + } + return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator; +} + +/** + * @brief Calculate offset pose. The offset values are defined in the local coordinate of the input + * pose. + */ +inline geometry_msgs::msg::Pose calcOffsetPose( + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Transform transform; + transform.translation = createTranslation(x, y, z); + transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0); + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(p, tf_pose); + tf2::toMsg(tf_pose * tf_offset, pose); + return pose; +} +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/geometry/pose_deviation.hpp b/common/autoware_utils/include/autoware_utils/geometry/pose_deviation.hpp new file mode 100644 index 0000000000000..7f5d02076c77b --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/geometry/pose_deviation.hpp @@ -0,0 +1,85 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#define AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/normalization.hpp" + +#include + +namespace autoware_utils +{ +struct PoseDeviation +{ + double lateral; + double longitudinal; + double yaw; +}; + +inline double calcLateralDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) +{ + const auto & base_point = base_pose.position; + + const auto yaw = tf2::getYaw(base_pose.orientation); + const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; + + const auto dx = target_point.x - base_point.x; + const auto dy = target_point.y - base_point.y; + const Eigen::Vector3d diff_vec{dx, dy, 0}; + + const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec); + + return cross_vec.z(); +} + +inline double calcLongitudinalDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) +{ + const auto & base_point = base_pose.position; + + const auto yaw = tf2::getYaw(base_pose.orientation); + const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; + + const auto dx = target_point.x - base_point.x; + const auto dy = target_point.y - base_point.y; + const Eigen::Vector3d diff_vec{dx, dy, 0}; + + return base_unit_vec.dot(diff_vec); +} + +inline double calcYawDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) +{ + const auto base_yaw = tf2::getYaw(base_pose.orientation); + const auto target_yaw = tf2::getYaw(target_pose.orientation); + return normalizeRadian(target_yaw - base_yaw); +} + +inline PoseDeviation calcPoseDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) +{ + PoseDeviation deviation{}; + + deviation.lateral = calcLateralDeviation(base_pose, target_pose.position); + deviation.longitudinal = calcLongitudinalDeviation(base_pose, target_pose.position); + deviation.yaw = calcYawDeviation(base_pose, target_pose); + + return deviation; +} +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/constants.hpp b/common/autoware_utils/include/autoware_utils/math/constants.hpp new file mode 100644 index 0000000000000..6b350a9d27e32 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/math/constants.hpp @@ -0,0 +1,24 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#define AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ + +namespace autoware_utils +{ +constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20 +constexpr double gravity = 9.80665; +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/normalization.hpp b/common/autoware_utils/include/autoware_utils/math/normalization.hpp new file mode 100644 index 0000000000000..ec992468cf087 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/math/normalization.hpp @@ -0,0 +1,50 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#define AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ + +#include "autoware_utils/math/constants.hpp" + +#include + +namespace autoware_utils +{ +inline double normalizeDegree(const double deg, const double min_deg = -180) +{ + const auto max_deg = min_deg + 360.0; + + const auto value = std::fmod(deg, 360.0); + if (min_deg <= value && value < max_deg) { + return value; + } + + return value - std::copysign(360.0, value); +} + +inline double normalizeRadian(const double rad, const double min_rad = -pi) +{ + const auto max_rad = min_rad + 2 * pi; + + const auto value = std::fmod(rad, 2 * pi); + if (min_rad <= value && value < max_rad) { + return value; + } + + return value - std::copysign(2 * pi, value); +} + +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/range.hpp b/common/autoware_utils/include/autoware_utils/math/range.hpp new file mode 100644 index 0000000000000..50ea769f9e8ee --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/math/range.hpp @@ -0,0 +1,78 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__MATH__RANGE_HPP_ +#define AUTOWARE_UTILS__MATH__RANGE_HPP_ + +#include +#include +#include +#include + +namespace autoware_utils +{ +template +std::vector arange(const T start, const T stop, const T step = 1) +{ + if (step == 0) { + throw std::invalid_argument("step must be non-zero value."); + } + + if (step > 0 && stop < start) { + throw std::invalid_argument("must be stop >= start for positive step."); + } + + if (step < 0 && stop > start) { + throw std::invalid_argument("must be stop <= start for negative step."); + } + + const double max_i_double = std::ceil(static_cast(stop - start) / step); + const auto max_i = static_cast(max_i_double); + + std::vector out; + out.reserve(max_i); + for (size_t i = 0; i < max_i; ++i) { + out.push_back(start + i * step); + } + + return out; +} + +template +std::vector linspace(const T start, const T stop, const size_t num) +{ + const auto start_double = static_cast(start); + const auto stop_double = static_cast(stop); + + if (num == 0) { + return {}; + } + + if (num == 1) { + return {start_double}; + } + + std::vector out; + out.reserve(num); + const double step = (stop_double - start_double) / static_cast(num - 1); + for (size_t i = 0; i < num; i++) { + out.push_back(start_double + static_cast(i) * step); + } + + return out; +} + +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__MATH__RANGE_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/unit_conversion.hpp b/common/autoware_utils/include/autoware_utils/math/unit_conversion.hpp new file mode 100644 index 0000000000000..e4bfc992a136b --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/math/unit_conversion.hpp @@ -0,0 +1,29 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#define AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ + +#include "autoware_utils/math/constants.hpp" + +namespace autoware_utils +{ +constexpr double deg2rad(const double deg) { return deg * pi / 180.0; } +constexpr double rad2deg(const double rad) { return rad * 180.0 / pi; } + +constexpr double kmph2mps(const double kmph) { return kmph * 1000.0 / 3600.0; } +constexpr double mps2kmph(const double mps) { return mps * 3600.0 / 1000.0; } +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/planning/planning_marker_helper.hpp b/common/autoware_utils/include/autoware_utils/planning/planning_marker_helper.hpp new file mode 100644 index 0000000000000..cb0448ee08e35 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/planning/planning_marker_helper.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ +#define AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ + +#include "autoware_utils/ros/marker_helper.hpp" + +#include + +namespace autoware_utils +{ +visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id); + +visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id); + +visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id); +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp b/common/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp new file mode 100644 index 0000000000000..bd2fd5641c6d5 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp @@ -0,0 +1,77 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#define AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ + +#include "autoware_utils/ros/debug_traits.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace autoware_utils +{ +namespace debug_publisher +{ +template < + class T_msg, class T, + std::enable_if_t::value, std::nullptr_t> = + nullptr> +T_msg toDebugMsg(const T & data, const rclcpp::Time & stamp) +{ + T_msg msg; + msg.stamp = stamp; + msg.data = data; + return msg; +} +} // namespace debug_publisher + +class DebugPublisher +{ +public: + explicit DebugPublisher(rclcpp::Node * node, const char * ns) : node_(node), ns_(ns) {} + + template < + class T, + std::enable_if_t::value, std::nullptr_t> = nullptr> + void publish(const std::string & name, const T & data, const rclcpp::QoS & qos = rclcpp::QoS(1)) + { + if (pub_map_.count(name) == 0) { + pub_map_[name] = node_->create_publisher(std::string(ns_) + "/" + name, qos); + } + + std::dynamic_pointer_cast>(pub_map_.at(name))->publish(data); + } + + template < + class T_msg, class T, + std::enable_if_t::value, std::nullptr_t> = nullptr> + void publish(const std::string & name, const T & data, const rclcpp::QoS & qos = rclcpp::QoS(1)) + { + publish(name, debug_publisher::toDebugMsg(data, node_->now()), qos); + } + +private: + rclcpp::Node * node_; + const char * ns_; + std::unordered_map> pub_map_; +}; +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/debug_traits.hpp b/common/autoware_utils/include/autoware_utils/ros/debug_traits.hpp new file mode 100644 index 0000000000000..9ca59e8f096f9 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/debug_traits.hpp @@ -0,0 +1,89 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#define AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace autoware_utils::debug_traits +{ +template +struct is_debug_message : std::false_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; +} // namespace autoware_utils::debug_traits + +#endif // AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/marker_helper.hpp b/common/autoware_utils/include/autoware_utils/ros/marker_helper.hpp new file mode 100644 index 0000000000000..cffb05e9239e2 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/marker_helper.hpp @@ -0,0 +1,99 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#define AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ + +#include + +#include + +#include + +namespace autoware_utils +{ +inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; +} + +inline geometry_msgs::msg::Quaternion createMarkerOrientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + return quaternion; +} + +inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +inline visualization_msgs::msg::Marker createDefaultMarker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = true; + + return marker; +} + +inline void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + } +} +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp b/common/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp new file mode 100644 index 0000000000000..2da2fa54defab --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp @@ -0,0 +1,67 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#define AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ + +#include + +#include + +#include +#include +#include + +namespace autoware_utils +{ +class ProcessingTimePublisher +{ +public: + explicit ProcessingTimePublisher( + rclcpp::Node * node, const std::string & name = "~/debug/processing_time_ms", + const rclcpp::QoS & qos = rclcpp::QoS(1)) + { + pub_processing_time_ = + node->create_publisher(name, qos); + } + + void publish(const std::map & processing_time_map) + { + diagnostic_msgs::msg::DiagnosticStatus status; + + for (const auto & m : processing_time_map) { + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = m.first; + key_value.value = to_string_with_precision(m.second, 3); + status.values.push_back(key_value); + } + + pub_processing_time_->publish(status); + } + +private: + rclcpp::Publisher::SharedPtr pub_processing_time_; + + template + std::string to_string_with_precision(const T & value, const int precision) + { + std::ostringstream oss; + oss.precision(precision); + oss << std::fixed << value; + return oss.str(); + } +}; +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/self_pose_listener.hpp b/common/autoware_utils/include/autoware_utils/ros/self_pose_listener.hpp new file mode 100644 index 0000000000000..27f1e5a86b466 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/self_pose_listener.hpp @@ -0,0 +1,58 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#define AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/transform_listener.hpp" + +#include + +#include + +namespace autoware_utils +{ +class SelfPoseListener +{ +public: + explicit SelfPoseListener(rclcpp::Node * node) : transform_listener_(node) {} + + void waitForFirstPose() + { + while (rclcpp::ok()) { + if (getCurrentPose()) { + return; + } + RCLCPP_INFO(transform_listener_.getLogger(), "waiting for self pose..."); + rclcpp::Rate(0.2).sleep(); + } + } + + geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose() + { + const auto tf = transform_listener_.getLatestTransform("map", "base_link"); + if (!tf) { + return {}; + } + + return std::make_shared(transform2pose(*tf)); + } + +private: + TransformListener transform_listener_; +}; +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/transform_listener.hpp b/common/autoware_utils/include/autoware_utils/ros/transform_listener.hpp new file mode 100644 index 0000000000000..a18ce2a207d50 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/transform_listener.hpp @@ -0,0 +1,85 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#define AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ + +#include + +#include + +#include +#include +#include + +#include +#include + +namespace autoware_utils +{ +class TransformListener +{ +public: + explicit TransformListener(rclcpp::Node * node) + : clock_(node->get_clock()), logger_(node->get_logger()) + { + tf_buffer_ = std::make_shared(clock_); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), node->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + geometry_msgs::msg::TransformStamped::ConstSharedPtr getLatestTransform( + const std::string & from, const std::string & to) + { + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_->lookupTransform(from, to, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + return {}; + } + + return std::make_shared(tf); + } + + geometry_msgs::msg::TransformStamped::ConstSharedPtr getTransform( + const std::string & from, const std::string & to, const rclcpp::Time & time, + const rclcpp::Duration & duration) + { + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_->lookupTransform(from, to, time, duration); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + return {}; + } + + return std::make_shared(tf); + } + + rclcpp::Logger getLogger() { return logger_; } + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/update_param.hpp b/common/autoware_utils/include/autoware_utils/ros/update_param.hpp new file mode 100644 index 0000000000000..4fce3e06530bc --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/update_param.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#define AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ + +#include + +#include +#include + +namespace autoware_utils +{ +template +bool updateParam(const std::vector & params, const std::string & name, T & value) +{ + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + + // Not found + if (itr == params.cend()) { + return false; + } + + value = itr->template get_value(); + return true; +} +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp b/common/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp new file mode 100644 index 0000000000000..6f8616b3a502d --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp @@ -0,0 +1,52 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#define AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ + +#include + +#include +#include +#include + +namespace autoware_utils +{ +template +T waitForParam( + rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name) +{ + std::chrono::seconds sec(1); + + auto param_client = std::make_shared(node, remote_node_name); + + while (!param_client->wait_for_service(sec)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service."); + return {}; + } + RCLCPP_INFO_THROTTLE( + node->get_logger(), *node->get_clock(), 1000 /* ms */, "waiting for node: %s, param: %s\n", + remote_node_name.c_str(), param_name.c_str()); + } + + if (param_client->has_parameter(param_name)) { + return param_client->get_parameter(param_name); + } + + return {}; +} +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/system/heartbeat_checker.hpp b/common/autoware_utils/include/autoware_utils/system/heartbeat_checker.hpp new file mode 100644 index 0000000000000..d197c18d7833d --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/system/heartbeat_checker.hpp @@ -0,0 +1,58 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ +#define AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ + +#include + +#include + +template +class HeaderlessHeartbeatChecker +{ +public: + HeaderlessHeartbeatChecker( + rclcpp::Node & node, const std::string & topic_name, const double timeout) + : clock_(node.get_clock()), timeout_(timeout) + { + using std::placeholders::_1; + sub_heartbeat_ = node.create_subscription( + topic_name, rclcpp::QoS{1}, std::bind(&HeaderlessHeartbeatChecker::onHeartbeat, this, _1)); + } + + bool isTimeout() + { + const auto time_from_last_heartbeat = clock_->now() - last_heartbeat_time_; + return time_from_last_heartbeat.seconds() > timeout_; + } + +private: + // Clock + rclcpp::Clock::SharedPtr clock_; + + // Parameter + double timeout_; + + // Subscriber + typename rclcpp::Subscription::SharedPtr sub_heartbeat_; + rclcpp::Time last_heartbeat_time_ = rclcpp::Time(0); + + void onHeartbeat([[maybe_unused]] const typename HeartbeatMsg::ConstSharedPtr msg) + { + last_heartbeat_time_ = clock_->now(); + } +}; + +#endif // AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/system/stop_watch.hpp b/common/autoware_utils/include/autoware_utils/system/stop_watch.hpp new file mode 100644 index 0000000000000..bde2bdc727bd8 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/system/stop_watch.hpp @@ -0,0 +1,63 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#define AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ + +#include +#include +#include + +namespace autoware_utils +{ +template < + class OutputUnit = std::chrono::seconds, class InternalUnit = std::chrono::microseconds, + class Clock = std::chrono::steady_clock> +class StopWatch +{ +public: + StopWatch() { tic(default_name); } + + void tic(const std::string & name = default_name) { t_start_[name] = Clock::now(); } + + void tic(const char * name) { tic(std::string(name)); } + + double toc(const std::string & name, const bool reset = false) + { + const auto t_start = t_start_.at(name); + const auto t_end = Clock::now(); + const auto duration = std::chrono::duration_cast(t_end - t_start).count(); + + if (reset) { + t_start_[name] = Clock::now(); + } + + const auto one_sec = std::chrono::duration_cast(OutputUnit(1)).count(); + + return static_cast(duration) / one_sec; + } + + double toc(const char * name, const bool reset = false) { return toc(std::string(name), reset); } + + double toc(const bool reset = false) { return toc(default_name, reset); } + +private: + using Time = std::chrono::time_point; + static constexpr const char * default_name{"__auto__"}; + + std::unordered_map t_start_; +}; +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/trajectory/tmp_conversion.hpp b/common/autoware_utils/include/autoware_utils/trajectory/tmp_conversion.hpp new file mode 100644 index 0000000000000..ba745d41d8104 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/trajectory/tmp_conversion.hpp @@ -0,0 +1,69 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ +#define AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include + +#include +#include + +namespace autoware_utils +{ +/** + * @brief Convert std::vector to + * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory) +{ + autoware_auto_planning_msgs::msg::Trajectory output{}; + for (const auto & pt : trajectory) { + output.points.push_back(pt); + if (output.points.size() >= output.CAPACITY) { + break; + } + } + return output; +} + +/** + * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +{ + std::vector output(trajectory.points.size()); + std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); + return output; +} + +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/trajectory/trajectory.hpp b/common/autoware_utils/include/autoware_utils/trajectory/trajectory.hpp new file mode 100644 index 0000000000000..ae2c572eee2a7 --- /dev/null +++ b/common/autoware_utils/include/autoware_utils/trajectory/trajectory.hpp @@ -0,0 +1,280 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#define AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" + +#include + +#include +#include +#include + +namespace autoware_utils +{ +template +void validateNonEmpty(const T & points) +{ + if (points.empty()) { + throw std::invalid_argument("Points is empty."); + } +} + +template +boost::optional searchZeroVelocityIndex( + const T & points_with_twist, const size_t src_idx, const size_t dst_idx) +{ + validateNonEmpty(points_with_twist); + + constexpr double epsilon = 1e-3; + for (size_t i = src_idx; i < dst_idx; ++i) { + if (std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) < epsilon) { + return i; + } + } + + return {}; +} + +template +boost::optional searchZeroVelocityIndex(const T & points_with_twist) +{ + return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); +} + +template +size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & point) +{ + validateNonEmpty(points); + + double min_dist = std::numeric_limits::max(); + size_t min_idx = 0; + + for (size_t i = 0; i < points.size(); ++i) { + const auto dist = calcSquaredDistance2d(points.at(i), point); + if (dist < min_dist) { + min_dist = dist; + min_idx = i; + } + } + return min_idx; +} + +template +boost::optional findNearestIndex( + const T & points, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + validateNonEmpty(points); + + const double max_squared_dist = max_dist * max_dist; + + double min_squared_dist = std::numeric_limits::max(); + bool is_nearest_found = false; + size_t min_idx = 0; + + for (size_t i = 0; i < points.size(); ++i) { + const auto squared_dist = calcSquaredDistance2d(points.at(i), pose); + if (squared_dist > max_squared_dist) { + continue; + } + + const auto yaw = calcYawDeviation(getPose(points.at(i)), pose); + if (std::fabs(yaw) > max_yaw) { + continue; + } + + if (squared_dist >= min_squared_dist) { + continue; + } + + min_squared_dist = squared_dist; + min_idx = i; + is_nearest_found = true; + } + return is_nearest_found ? boost::optional(min_idx) : boost::none; +} + +/** + * @brief calculate longitudinal offset (length along trajectory from seg_idx point to nearest point + * to p_target on trajectory) If seg_idx point is after that nearest point, length is negative + * @param points points of trajectory, path, ... + * @param seg_idx segment index of point at beginning of length + * @param p_target target point at end of length + * @return signed length + */ +template +double calcLongitudinalOffsetToSegment( + const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target) +{ + validateNonEmpty(points); + + const auto p_front = getPoint(points.at(seg_idx)); + const auto p_back = getPoint(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}; + + if (segment_vec.norm() == 0.0) { + throw std::runtime_error("Same points are given."); + } + + return segment_vec.dot(target_vec) / segment_vec.norm(); +} + +/** + * @brief find nearest segment index to point + * segment is straight path between two continuous points of trajectory + * When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory + * @param point point to which to find nearest segment index + * @return nearest index + */ +template +size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point & point) +{ + const size_t nearest_idx = findNearestIndex(points, point); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +/** + * @brief calculate lateral offset from p_target (length from p_target to trajectory) + * If seg_idx point is after that nearest point, length is negative + * @param points points of trajectory, path, ... + * @param p_target target point + * @return length (unsigned) + */ +template +double calcLateralOffset(const T & points, const geometry_msgs::msg::Point & p_target) +{ + validateNonEmpty(points); + + const size_t seg_idx = findNearestSegmentIndex(points, p_target); + + const auto p_front = getPoint(points.at(seg_idx)); + const auto p_back = getPoint(points.at(seg_idx + 1)); + + 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}; + + if (segment_vec.norm() == 0.0) { + throw std::runtime_error("Same points are given."); + } + + const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); + return cross_vec(2) / segment_vec.norm(); +} + +/** + * @brief calcSignedArcLength from index to index + */ +template +double calcSignedArcLength(const T & points, const size_t src_idx, const size_t dst_idx) +{ + validateNonEmpty(points); + + if (src_idx > dst_idx) { + return -calcSignedArcLength(points, dst_idx, src_idx); + } + + double dist_sum = 0.0; + for (size_t i = src_idx; i < dst_idx; ++i) { + dist_sum += calcDistance2d(points.at(i), points.at(i + 1)); + } + return dist_sum; +} + +/** + * @brief calcSignedArcLength from point to index + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx) +{ + validateNonEmpty(points); + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return signed_length_on_traj - signed_length_src_offset; +} + +/** + * @brief calcSignedArcLength from index to point + */ +template +double calcSignedArcLength( + const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point) +{ + validateNonEmpty(points); + + return -calcSignedArcLength(points, dst_point, src_idx); +} + +/** + * @brief calcSignedArcLength from point to point + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, + const geometry_msgs::msg::Point & dst_point) +{ + validateNonEmpty(points); + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + +/** + * @brief calcArcLength for the whole length + */ +template +double calcArcLength(const T & points) +{ + validateNonEmpty(points); + + return calcSignedArcLength(points, 0, points.size() - 1); +} +} // namespace autoware_utils + +#endif // AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_utils/package.xml b/common/autoware_utils/package.xml new file mode 100644 index 0000000000000..ef2d09fa7ec9c --- /dev/null +++ b/common/autoware_utils/package.xml @@ -0,0 +1,30 @@ + + + autoware_utils + 0.1.0 + The autoware_utils package + Takamasa Horibe + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_planning_msgs + autoware_debug_msgs + builtin_interfaces + diagnostic_msgs + geometry_msgs + libboost-dev + rclcpp + tf2 + tf2_geometry_msgs + visualization_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_utils/src/autoware_utils.cpp b/common/autoware_utils/src/autoware_utils.cpp new file mode 100644 index 0000000000000..2458e2ce146ea --- /dev/null +++ b/common/autoware_utils/src/autoware_utils.cpp @@ -0,0 +1,15 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/autoware_utils.hpp" diff --git a/common/autoware_utils/src/planning/planning_marker_helper.cpp b/common/autoware_utils/src/planning/planning_marker_helper.cpp new file mode 100644 index 0000000000000..d98337a636080 --- /dev/null +++ b/common/autoware_utils/src/planning/planning_marker_helper.cpp @@ -0,0 +1,87 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/planning/planning_marker_helper.hpp" + +#include + +using autoware_utils::createDefaultMarker; +using autoware_utils::createMarkerColor; +using autoware_utils::createMarkerScale; + +namespace +{ +inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Virtual Wall + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(0.1, 5.0, 2.0), color); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 1.0; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +} // namespace + +namespace autoware_utils +{ +visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id) +{ + return createVirtualWallMarkerArray( + pose, module_name, "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id) +{ + return createVirtualWallMarkerArray( + pose, module_name, "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id) +{ + return createVirtualWallMarkerArray( + pose, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); +} +} // namespace autoware_utils diff --git a/common/autoware_utils/test/src/geometry/test_boost_geometry.cpp b/common/autoware_utils/test/src/geometry/test_boost_geometry.cpp new file mode 100644 index 0000000000000..6e96855e4289d --- /dev/null +++ b/common/autoware_utils/test/src/geometry/test_boost_geometry.cpp @@ -0,0 +1,83 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/geometry/boost_geometry.hpp" + +#include + +#include + +namespace bg = boost::geometry; + +using autoware_utils::Point2d; +using autoware_utils::Point3d; + +TEST(boost_geometry, boost_geometry_distance) +{ + { + const Point2d p1(1.0, 2.0); + const Point2d p2(2.0, 4.0); + EXPECT_DOUBLE_EQ(bg::distance(p1, p2), std::sqrt(5)); + } + + { + const Point3d p1(1.0, 2.0, 3.0); + const Point3d p2(2.0, 4.0, 6.0); + EXPECT_DOUBLE_EQ(bg::distance(p1, p2), std::sqrt(14)); + } +} + +TEST(boost_geometry, to_3d) +{ + const Point2d p_2d(1.0, 2.0); + const Point3d p_3d(1.0, 2.0, 3.0); + EXPECT_TRUE(p_2d.to_3d(3.0) == p_3d); +} + +TEST(boost_geometry, to_2d) +{ + const Point2d p_2d(1.0, 2.0); + const Point3d p_3d(1.0, 2.0, 3.0); + EXPECT_TRUE(p_3d.to_2d() == p_2d); +} + +TEST(boost_geometry, toMsg) +{ + using autoware_utils::toMsg; + + { + const Point3d p(1.0, 2.0, 3.0); + const geometry_msgs::msg::Point p_msg = toMsg(p); + + EXPECT_DOUBLE_EQ(p_msg.x, 1.0); + EXPECT_DOUBLE_EQ(p_msg.y, 2.0); + EXPECT_DOUBLE_EQ(p_msg.z, 3.0); + } +} + +TEST(boost_geometry, fromMsg) +{ + using autoware_utils::fromMsg; + + geometry_msgs::msg::Point p_msg; + p_msg.x = 1.0; + p_msg.y = 2.0; + p_msg.z = 3.0; + + const Point3d p = fromMsg(p_msg); + + EXPECT_DOUBLE_EQ(p.x(), 1.0); + EXPECT_DOUBLE_EQ(p.y(), 2.0); + EXPECT_DOUBLE_EQ(p.z(), 3.0); +} diff --git a/common/autoware_utils/test/src/geometry/test_geometry.cpp b/common/autoware_utils/test/src/geometry/test_geometry.cpp new file mode 100644 index 0000000000000..99ae3ac32dfa8 --- /dev/null +++ b/common/autoware_utils/test/src/geometry/test_geometry.cpp @@ -0,0 +1,807 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" + +#include + +#include + +constexpr double epsilon = 1e-6; + +TEST(geometry, getPoint) +{ + using autoware_utils::getPoint; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + + { + struct AnyPoint + { + double x; + double y; + double z; + }; + const AnyPoint p{x_ans, y_ans, z_ans}; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::Point p; + p.x = x_ans; + p.y = y_ans; + p.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose.position.x = x_ans; + p.pose.pose.position.y = y_ans; + p.pose.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + autoware_auto_planning_msgs::msg::PathPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + autoware_auto_planning_msgs::msg::TrajectoryPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } +} + +TEST(geometry, getPose) +{ + using autoware_utils::getPose; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + const double q_x_ans = 0.1; + const double q_y_ans = 0.2; + const double q_z_ans = 0.3; + const double q_w_ans = 0.4; + + { + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + p.orientation.x = q_x_ans; + p.orientation.y = q_y_ans; + p.orientation.z = q_z_ans; + p.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + p.pose.orientation.x = q_x_ans; + p.pose.orientation.y = q_y_ans; + p.pose.orientation.z = q_z_ans; + p.pose.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + autoware_auto_planning_msgs::msg::PathPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + p.pose.orientation.x = q_x_ans; + p.pose.orientation.y = q_y_ans; + p.pose.orientation.z = q_z_ans; + p.pose.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + autoware_auto_planning_msgs::msg::TrajectoryPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + p.pose.orientation.x = q_x_ans; + p.pose.orientation.y = q_y_ans; + p.pose.orientation.z = q_z_ans; + p.pose.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } +} + +TEST(geometry, createPoint) +{ + using autoware_utils::createPoint; + + const geometry_msgs::msg::Point p_out = createPoint(1.0, 2.0, 3.0); + EXPECT_DOUBLE_EQ(p_out.x, 1.0); + EXPECT_DOUBLE_EQ(p_out.y, 2.0); + EXPECT_DOUBLE_EQ(p_out.z, 3.0); +} + +TEST(geometry, createQuaternion) +{ + using autoware_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 = + createQuaternion(0.18257419, 0.36514837, 0.54772256, 0.73029674); + EXPECT_DOUBLE_EQ(q_out.x, 0.18257419); + EXPECT_DOUBLE_EQ(q_out.y, 0.36514837); + EXPECT_DOUBLE_EQ(q_out.z, 0.54772256); + EXPECT_DOUBLE_EQ(q_out.w, 0.73029674); +} + +TEST(geometry, createTranslation) +{ + using autoware_utils::createTranslation; + + const geometry_msgs::msg::Vector3 v_out = createTranslation(1.0, 2.0, 3.0); + EXPECT_DOUBLE_EQ(v_out.x, 1.0); + EXPECT_DOUBLE_EQ(v_out.y, 2.0); + EXPECT_DOUBLE_EQ(v_out.z, 3.0); +} + +TEST(geometry, createQuaternionFromRPY) +{ + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + + { + const auto q_out = createQuaternionFromRPY(0, 0, 0); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.0); + EXPECT_DOUBLE_EQ(q_out.w, 1.0); + } + + { + const auto q_out = createQuaternionFromRPY(0, 0, deg2rad(90)); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757); + EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757); + } + + { + const auto q_out = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + EXPECT_DOUBLE_EQ(q_out.x, 0.17677669529663687); + EXPECT_DOUBLE_EQ(q_out.y, 0.30618621784789724); + EXPECT_DOUBLE_EQ(q_out.z, 0.17677669529663692); + EXPECT_DOUBLE_EQ(q_out.w, 0.91855865354369193); + } +} + +TEST(geometry, createQuaternionFromYaw) +{ + using autoware_utils::createQuaternionFromYaw; + using autoware_utils::deg2rad; + + { + const auto q_out = createQuaternionFromYaw(0); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.0); + EXPECT_DOUBLE_EQ(q_out.w, 1.0); + } + + { + const auto q_out = createQuaternionFromYaw(deg2rad(90)); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757); + EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757); + } + + { + const auto q_out = createQuaternionFromYaw(deg2rad(30)); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.25881904510252074); + EXPECT_DOUBLE_EQ(q_out.w, 0.96592582628906831); + } +} + +TEST(geometry, calcElevationAngle) +{ + using autoware_utils::calcElevationAngle; + using autoware_utils::createPoint; + using autoware_utils::deg2rad; + + { + const auto p1 = createPoint(1.0, 1.0, 1.0); + const auto p2 = createPoint(1.0, 1.0, -10.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-90.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, -std::sqrt(3.0)); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-60.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, -1.0); + const auto p2 = createPoint(0.0, 1.0, -2.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 1.0); + const auto p2 = createPoint(1.0, 1.0, 1.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(0.0), epsilon); + } + { + const auto p1 = createPoint(-100, -100, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(0.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 1.0); + const auto p2 = createPoint(0.0, 1.0, 2.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, std::sqrt(3.0)); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(60.0), epsilon); + } + { + const auto p1 = createPoint(1.0, 1.0, 1.0); + const auto p2 = createPoint(1.0, 1.0, 10.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(90.0), epsilon); + } +} + +TEST(geometry, calcAzimuthAngle) +{ + using autoware_utils::calcAzimuthAngle; + using autoware_utils::createPoint; + using autoware_utils::deg2rad; + + { + const auto p1 = createPoint(0.0, 0.0, 9.0); + const auto p2 = createPoint(-100, -epsilon, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-180.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 2.0); + const auto p2 = createPoint(-1.0, -1.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-135.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 10.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 6.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-90.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, -1.0, 4.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 1.0, 3.3); + const auto p2 = createPoint(10.0, 1.0, -10.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(0.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 2.0); + const auto p2 = createPoint(1.0, 1.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 10.0); + const auto p2 = createPoint(0.0, 10.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(90.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 2.0); + const auto p2 = createPoint(-1.0, 1.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(135.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 9.0); + const auto p2 = createPoint(-100, epsilon, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(180.0), epsilon); + } +} + +TEST(geometry, calcDistance2d) +{ + using autoware_utils::calcDistance2d; + + geometry_msgs::msg::Point point; + point.x = 1.0; + point.y = 2.0; + point.z = 3.0; + + geometry_msgs::msg::Pose pose; + pose.position.x = 5.0; + pose.position.y = 5.0; + pose.position.z = 4.0; + + EXPECT_DOUBLE_EQ(calcDistance2d(point, pose), 5.0); +} + +TEST(geometry, calcSquaredDistance2d) +{ + using autoware_utils::calcSquaredDistance2d; + + geometry_msgs::msg::Point point; + point.x = 1.0; + point.y = 2.0; + point.z = 3.0; + + geometry_msgs::msg::Pose pose; + pose.position.x = 5.0; + pose.position.y = 5.0; + pose.position.z = 4.0; + + EXPECT_DOUBLE_EQ(calcSquaredDistance2d(point, pose), 25.0); +} + +TEST(geometry, calcDistance3d) +{ + using autoware_utils::calcDistance3d; + + geometry_msgs::msg::Point point; + point.x = 1.0; + point.y = 2.0; + point.z = 3.0; + + geometry_msgs::msg::Pose pose; + pose.position.x = 3.0; + pose.position.y = 4.0; + pose.position.z = 4.0; + + EXPECT_DOUBLE_EQ(calcDistance3d(point, pose), 3.0); +} + +TEST(geometry, getRPY) +{ + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + using autoware_utils::getRPY; + + { + const double ans_roll = deg2rad(5); + const double ans_pitch = deg2rad(10); + const double ans_yaw = deg2rad(15); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + const auto rpy = getRPY(quat); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + { + const double ans_roll = deg2rad(0); + const double ans_pitch = deg2rad(5); + const double ans_yaw = deg2rad(-10); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + const auto rpy = getRPY(quat); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + { + const double ans_roll = deg2rad(30); + const double ans_pitch = deg2rad(-20); + const double ans_yaw = deg2rad(0); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + const auto rpy = getRPY(quat); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } +} + +TEST(geometry, getRPY_wrapper) +{ + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + using autoware_utils::getRPY; + + { + const double ans_roll = deg2rad(45); + const double ans_pitch = deg2rad(25); + const double ans_yaw = deg2rad(-5); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + + // geometry_msgs::Pose + { + geometry_msgs::msg::Pose pose{}; + pose.orientation = quat; + const auto rpy = getRPY(pose); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + // geometry_msgs::PoseStamped + { + geometry_msgs::msg::PoseStamped pose{}; + pose.pose.orientation = quat; + const auto rpy = getRPY(pose); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + // geometry_msgs::PoseWithCovarianceStamped + { + geometry_msgs::msg::PoseWithCovarianceStamped pose{}; + pose.pose.pose.orientation = quat; + const auto rpy = getRPY(pose); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + } +} + +TEST(geometry, transform2pose) +{ + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + using autoware_utils::transform2pose; + + { + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Pose pose = transform2pose(transform); + + EXPECT_DOUBLE_EQ(transform.translation.x, pose.position.x); + EXPECT_DOUBLE_EQ(transform.translation.y, pose.position.y); + EXPECT_DOUBLE_EQ(transform.translation.z, pose.position.z); + EXPECT_DOUBLE_EQ(transform.rotation.x, pose.orientation.x); + EXPECT_DOUBLE_EQ(transform.rotation.y, pose.orientation.y); + EXPECT_DOUBLE_EQ(transform.rotation.z, pose.orientation.z); + EXPECT_DOUBLE_EQ(transform.rotation.w, pose.orientation.w); + } + + { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.frame_id = "test"; + transform_stamped.header.stamp = rclcpp::Time(2.0); + transform_stamped.transform.translation.x = 1.0; + transform_stamped.transform.translation.y = 2.0; + transform_stamped.transform.translation.z = 3.0; + transform_stamped.transform.rotation = + createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::PoseStamped pose_stamped = transform2pose(transform_stamped); + + EXPECT_EQ(transform_stamped.header.frame_id, pose_stamped.header.frame_id); + EXPECT_DOUBLE_EQ( + rclcpp::Time(transform_stamped.header.stamp).seconds(), + rclcpp::Time(pose_stamped.header.stamp).seconds()); + + EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.x, pose_stamped.pose.position.x); + EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.y, pose_stamped.pose.position.y); + EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.z, pose_stamped.pose.position.z); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.x, pose_stamped.pose.orientation.x); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.y, pose_stamped.pose.orientation.y); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.z, pose_stamped.pose.orientation.z); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.w, pose_stamped.pose.orientation.w); + } +} + +TEST(geometry, pose2transform) +{ + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + using autoware_utils::pose2transform; + + { + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = 2.0; + pose.position.z = 3.0; + pose.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Transform transform = pose2transform(pose); + + EXPECT_DOUBLE_EQ(pose.position.x, transform.translation.x); + EXPECT_DOUBLE_EQ(pose.position.y, transform.translation.y); + EXPECT_DOUBLE_EQ(pose.position.z, transform.translation.z); + EXPECT_DOUBLE_EQ(pose.orientation.x, transform.rotation.x); + EXPECT_DOUBLE_EQ(pose.orientation.y, transform.rotation.y); + EXPECT_DOUBLE_EQ(pose.orientation.z, transform.rotation.z); + EXPECT_DOUBLE_EQ(pose.orientation.w, transform.rotation.w); + } + + { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.frame_id = "test"; + pose_stamped.header.stamp = rclcpp::Time(2.0); + pose_stamped.pose.position.x = 1.0; + pose_stamped.pose.position.y = 2.0; + pose_stamped.pose.position.z = 3.0; + pose_stamped.pose.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + const std::string child_frame_id = "child"; + + const geometry_msgs::msg::TransformStamped transform_stamped = + pose2transform(pose_stamped, child_frame_id); + + EXPECT_EQ(pose_stamped.header.frame_id, transform_stamped.header.frame_id); + EXPECT_EQ(child_frame_id, transform_stamped.child_frame_id); + EXPECT_DOUBLE_EQ( + rclcpp::Time(pose_stamped.header.stamp).seconds(), + rclcpp::Time(transform_stamped.header.stamp).seconds()); + + EXPECT_DOUBLE_EQ(pose_stamped.pose.position.x, transform_stamped.transform.translation.x); + EXPECT_DOUBLE_EQ(pose_stamped.pose.position.y, transform_stamped.transform.translation.y); + EXPECT_DOUBLE_EQ(pose_stamped.pose.position.z, transform_stamped.transform.translation.z); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.x, transform_stamped.transform.rotation.x); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.y, transform_stamped.transform.rotation.y); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.z, transform_stamped.transform.rotation.z); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.w, transform_stamped.transform.rotation.w); + } +} + +TEST(geometry, transformPoint) +{ + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + using autoware_utils::Point2d; + using autoware_utils::Point3d; + using autoware_utils::transformPoint; + + { + const Point2d p(1.0, 2.0); + + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.rotation = createQuaternionFromRPY(0, 0, deg2rad(30)); + + const Point2d p_transformed = transformPoint(p, transform); + + EXPECT_DOUBLE_EQ(p_transformed.x(), 0.86602540378443882); + EXPECT_DOUBLE_EQ(p_transformed.y(), 4.2320508075688767); + } + + { + const Point3d p(1.0, 2.0, 3.0); + + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const Point3d p_transformed = transformPoint(p, transform); + + EXPECT_DOUBLE_EQ(p_transformed.x(), 3.1919872981077804); + EXPECT_DOUBLE_EQ(p_transformed.y(), 3.5334936490538906); + EXPECT_DOUBLE_EQ(p_transformed.z(), 5.6160254037844393); + } +} + +TEST(geometry, transformVector) +{ + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + using autoware_utils::MultiPoint3d; + using autoware_utils::transformVector; + + { + const MultiPoint3d ps{{1.0, 2.0, 3.0}, {2.0, 3.0, 4.0}}; + + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const MultiPoint3d ps_transformed = transformVector(ps, transform); + + EXPECT_DOUBLE_EQ(ps_transformed.at(0).x(), 3.1919872981077804); + EXPECT_DOUBLE_EQ(ps_transformed.at(0).y(), 3.5334936490538906); + EXPECT_DOUBLE_EQ(ps_transformed.at(0).z(), 5.6160254037844393); + + EXPECT_DOUBLE_EQ(ps_transformed.at(1).x(), 4.350480947161671); + EXPECT_DOUBLE_EQ(ps_transformed.at(1).y(), 4.625); + EXPECT_DOUBLE_EQ(ps_transformed.at(1).z(), 6.299038105676658); + } +} + +TEST(geometry, calcCurvature) +{ + using autoware_utils::calcCurvature; + // Straight Line + { + geometry_msgs::msg::Point p1 = autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_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 = autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_utils::createPoint(1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_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 = autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_utils::createPoint(5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_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 = autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_utils::createPoint(-1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_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 = autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_utils::createPoint(-5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware_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 = autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware_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)); + ASSERT_ANY_THROW(calcCurvature(p1, p2, p2)); + } +} + +TEST(geometry, calcOffsetPose) +{ + using autoware_utils::calcOffsetPose; + using autoware_utils::createPoint; + using autoware_utils::createQuaternion; + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(1.0, 2.0, 3.0); + p_in.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + const auto p_out = calcOffsetPose(p_in, 1.0, 1.0, 1.0); + + EXPECT_DOUBLE_EQ(p_out.position.x, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 3.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 4.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); + } + + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 3.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90)); + + const auto p_out = calcOffsetPose(p_in, 2.0, 1.0, 3.0); + + EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 5.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 4.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.70710678118654757); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.70710678118654757); + } + + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 1.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); + + const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0); + + EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729); + EXPECT_DOUBLE_EQ(p_out.position.y, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.25881904510252068); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831); + } +} diff --git a/common/autoware_utils/test/src/geometry/test_pose_deviation.cpp b/common/autoware_utils/test/src/geometry/test_pose_deviation.cpp new file mode 100644 index 0000000000000..8ce2e9448a7d1 --- /dev/null +++ b/common/autoware_utils/test/src/geometry/test_pose_deviation.cpp @@ -0,0 +1,42 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/math/unit_conversion.hpp" + +#include + +TEST(geometry, pose_deviation) +{ + using autoware_utils::calcPoseDeviation; + using autoware_utils::createQuaternionFromRPY; + using autoware_utils::deg2rad; + + geometry_msgs::msg::Pose pose1; + pose1.position.x = 1.0; + pose1.position.y = 2.0; + pose1.position.z = 3.0; + pose1.orientation = createQuaternionFromRPY(0, 0, deg2rad(45)); + + geometry_msgs::msg::Pose pose2; + pose2.position.x = 2.0; + pose2.position.y = 4.0; + pose2.position.z = 6.0; + pose2.orientation = createQuaternionFromRPY(0, 0, deg2rad(60)); + + const auto deviation = calcPoseDeviation(pose1, pose2); + EXPECT_DOUBLE_EQ(deviation.lateral, 0.70710678118654735); + EXPECT_DOUBLE_EQ(deviation.longitudinal, 2.1213203435596428); + EXPECT_DOUBLE_EQ(deviation.yaw, deg2rad(15)); +} diff --git a/common/autoware_utils/test/src/math/test_constants.cpp b/common/autoware_utils/test/src/math/test_constants.cpp new file mode 100644 index 0000000000000..9aeb0f6909194 --- /dev/null +++ b/common/autoware_utils/test/src/math/test_constants.cpp @@ -0,0 +1,31 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/math/constants.hpp" + +#include + +TEST(constants, pi) +{ + using autoware_utils::pi; + + EXPECT_DOUBLE_EQ(pi, 3.14159265358979323846); +} + +TEST(constants, gravity) +{ + using autoware_utils::gravity; + + EXPECT_DOUBLE_EQ(gravity, 9.80665); +} diff --git a/common/autoware_utils/test/src/math/test_normalization.cpp b/common/autoware_utils/test/src/math/test_normalization.cpp new file mode 100644 index 0000000000000..76ac7ec17d02a --- /dev/null +++ b/common/autoware_utils/test/src/math/test_normalization.cpp @@ -0,0 +1,83 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/math/normalization.hpp" + +#include + +TEST(normalization, normalizeDegree) +{ + using autoware_utils::normalizeDegree; + + // -180 <= deg < 180 + { + constexpr double eps = 0.1; + constexpr double v_min = -180; + constexpr double v_mid = 0; + constexpr double v_max = 180; + + EXPECT_DOUBLE_EQ(normalizeDegree(v_min - eps), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeDegree(v_min), v_min); + EXPECT_DOUBLE_EQ(normalizeDegree(v_mid), v_mid); + EXPECT_DOUBLE_EQ(normalizeDegree(v_max - eps), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeDegree(v_max), v_min); + } + + // 0 <= deg < 360 + { + constexpr double eps = 0.1; + constexpr double v_min = 0; + constexpr double v_mid = 180; + constexpr double v_max = 360; + + EXPECT_DOUBLE_EQ(normalizeDegree(v_min - eps, 0), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeDegree(v_min, 0), v_min); + EXPECT_DOUBLE_EQ(normalizeDegree(v_mid, 0), v_mid); + EXPECT_DOUBLE_EQ(normalizeDegree(v_max - eps, 0), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeDegree(v_max, 0), v_min); + } +} + +TEST(normalization, normalizeRadian) +{ + using autoware_utils::normalizeRadian; + + // -M_PI <= deg < M_PI + { + constexpr double eps = 0.1; + constexpr double v_min = -M_PI; + constexpr double v_mid = 0; + constexpr double v_max = M_PI; + + EXPECT_DOUBLE_EQ(normalizeRadian(v_min - eps), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeRadian(v_min), v_min); + EXPECT_DOUBLE_EQ(normalizeRadian(v_mid), v_mid); + EXPECT_DOUBLE_EQ(normalizeRadian(v_max - eps), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeRadian(v_max), v_min); + } + + // 0 <= deg < 2 * M_PI + { + constexpr double eps = 0.1; + constexpr double v_min = 0; + constexpr double v_mid = M_PI; + constexpr double v_max = 2 * M_PI; + + EXPECT_DOUBLE_EQ(normalizeRadian(v_min - eps, 0), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeRadian(v_min, 0), v_min); + EXPECT_DOUBLE_EQ(normalizeRadian(v_mid, 0), v_mid); + EXPECT_DOUBLE_EQ(normalizeRadian(v_max - eps, 0), v_max - eps); + EXPECT_DOUBLE_EQ(normalizeRadian(v_max, 0), v_min); + } +} diff --git a/common/autoware_utils/test/src/math/test_range.cpp b/common/autoware_utils/test/src/math/test_range.cpp new file mode 100644 index 0000000000000..2d166dbfa9044 --- /dev/null +++ b/common/autoware_utils/test/src/math/test_range.cpp @@ -0,0 +1,235 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/math/range.hpp" + +#include + +#include + +template +void expect_near_vector( + const std::vector & input, const std::vector & expect, const T abs_error = 1e-6) +{ + ASSERT_EQ(input.size(), expect.size()) << "unequal length"; + + for (size_t i = 0; i < input.size(); i++) { + const auto x = input.at(i); + const auto y = expect.at(i); + EXPECT_NEAR(x, y, abs_error) << "differ at index " << i << ":" << x << ", " << y; + } +} + +void expect_eq_vector(const std::vector & input, const std::vector & expect) +{ + ASSERT_EQ(input.size(), expect.size()) << "unequal length"; + + for (size_t i = 0; i < input.size(); ++i) { + const auto x = input.at(i); + const auto y = expect.at(i); + EXPECT_EQ(x, y) << "differ at index " << i << ": " << x << ", " << y; + } +} + +TEST(arange_Test, arange_double) +{ + using autoware_utils::arange; + + // general cases + { + expect_near_vector(arange(0.0, 5.0, 1.0), std::vector{0.0, 1.0, 2.0, 3.0, 4.0}); + expect_near_vector(arange(0.0, 5.1, 1.0), std::vector{0.0, 1.0, 2.0, 3.0, 4.0, 5.0}); + expect_near_vector(arange(2.0, 5.0, 0.5), std::vector{2.0, 2.5, 3.0, 3.5, 4.0, 4.5}); + expect_near_vector( + arange(0.1, 2.0, 0.2), std::vector{0.1, 0.3, 0.5, 0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 1.9}); + + // argument step is omitted. + expect_near_vector(arange(0.0, 5.0), std::vector{0.0, 1.0, 2.0, 3.0, 4.0}); + } + // corner cases + { + // stop == start + expect_near_vector(arange(3.0, 3.0, 1.0), std::vector{}); + // step > stop - start + expect_near_vector(arange(2.0, 4.0, 10.0), std::vector{2.0}); + // step is negative value and stop < start + expect_near_vector( + arange(3.0, -3.0, -1.0), std::vector{3.0, 2.0, 1.0, 0.0, -1.0, -2.0}); + } + // invalid cases + { + // step == 0 + EXPECT_THROW(arange(0.0, 5.0, 0.0), std::invalid_argument); + EXPECT_THROW(arange(0.0, -5.0, 0.0), std::invalid_argument); + + // positive step but start > stop. + EXPECT_THROW(arange(0.0, -1.0, 0.1), std::invalid_argument); + // negative step but start < stop. + EXPECT_THROW(arange(0.0, 1.0, -0.1), std::invalid_argument); + } +} + +TEST(arange_Test, arange_float) +{ + using autoware_utils::arange; + + // general cases + { + expect_near_vector(arange(0.0f, 5.0f, 1.0f), std::vector{0.0, 1.0, 2.0, 3.0, 4.0}); + expect_near_vector(arange(0.0f, 5.1f, 1.0f), std::vector{0.0, 1.0, 2.0, 3.0, 4.0, 5.0}); + expect_near_vector(arange(2.0f, 5.0f, 0.5f), std::vector{2.0, 2.5, 3.0, 3.5, 4.0, 4.5}); + expect_near_vector( + arange(0.1f, 2.0f, 0.2f), + std::vector{0.1, 0.3, 0.5, 0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 1.9}); + + // argument step is omitted. + expect_near_vector(arange(0.0f, 5.0f), std::vector{0.0, 1.0, 2.0, 3.0, 4.0}); + } + // corner cases + { + // stop == start + expect_near_vector(arange(3.0f, 3.0f, 1.0f), std::vector{}); + // step > stop - start + expect_near_vector(arange(2.0f, 4.0f, 10.0f), std::vector{2.0}); + // step is negative value and stop < start + expect_near_vector( + arange(3.0f, -3.0f, -1.0f), std::vector{3.0, 2.0, 1.0, 0.0, -1.0, -2.0}); + } + // invalid cases + { + // step == 0 + EXPECT_THROW(arange(0.0f, 5.0f, 0.0f), std::invalid_argument); + EXPECT_THROW(arange(0.0f, -5.0f, 0.0f), std::invalid_argument); + + // positive step but start > stop. + EXPECT_THROW(arange(0.0f, -1.0f, 0.1f), std::invalid_argument); + // negative step but start < stop. + EXPECT_THROW(arange(0.0f, 1.0f, -0.1f), std::invalid_argument); + } +} + +TEST(arange_Test, arange_int) +{ + using autoware_utils::arange; + + // general cases + { + expect_eq_vector(arange(0, 5, 2), std::vector{0, 2, 4}); + + // argument step is omitted. + expect_eq_vector(arange(0, 5), std::vector{0, 1, 2, 3, 4}); + } + // corner cases + { + // stop == start + expect_eq_vector(arange(3, 3, 1), std::vector{}); + // step > stop - start + expect_eq_vector(arange(2, 4, 10), std::vector{2}); + // step is negative value and stop < start + expect_eq_vector(arange(3, -3, -1), std::vector{3, 2, 1, 0, -1, -2}); + } + // invalid cases + { + // step == 0 + EXPECT_THROW(arange(0, 5, 0), std::invalid_argument); + EXPECT_THROW(arange(0, -5, 0), std::invalid_argument); + + // positive step but start > stop. + EXPECT_THROW(arange(0, -3, 1), std::invalid_argument); + // negative step but start < stop. + EXPECT_THROW(arange(0, 3, -1), std::invalid_argument); + } +} + +TEST(test_linspace, linspace_double) +{ + using autoware_utils::linspace; + + // general cases + { + expect_near_vector(linspace(3.0, 7.0, 5), std::vector{3.0, 4.0, 5.0, 6.0, 7.0}); + expect_near_vector(linspace(1.0, 2.0, 3), std::vector{1.0, 1.5, 2.0}); + expect_near_vector(linspace(-1.0, 3.0, 5), std::vector{-1.0, 0.0, 1.0, 2.0, 3.0}); + expect_near_vector( + linspace(0.1, 1.0, 10), + std::vector{0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}); + } + // corner cases + { + // num == 2 + expect_near_vector(linspace(0.0, 10.0, 2), std::vector{0.0, 10.0}); + // num == 1 + expect_near_vector(linspace(11.0, 20.0, 1), std::vector{11.0}); + // num == 0 + expect_near_vector(linspace(30.0, 40.0, 0), std::vector{}); + // start == stop + expect_near_vector(linspace(2.3, 2.3, 3), std::vector{2.3, 2.3, 2.3}); + // start > stop + expect_near_vector(linspace(10.0, 5.0, 6), std::vector{10.0, 9.0, 8.0, 7.0, 6.0, 5.0}); + } +} + +TEST(test_linspace, linspace_float) +{ + using autoware_utils::linspace; + + // general cases + { + expect_near_vector(linspace(3.0f, 7.0f, 5), std::vector{3.0, 4.0, 5.0, 6.0, 7.0}); + expect_near_vector(linspace(1.0f, 2.0f, 3), std::vector{1.0, 1.5, 2.0}); + expect_near_vector(linspace(-1.0f, 3.0f, 5), std::vector{-1.0, 0.0, 1.0, 2.0, 3.0}); + expect_near_vector( + linspace(0.1f, 1.0f, 10), + std::vector{0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}); + } + // corner cases + { + // num == 2 + expect_near_vector(linspace(0.0f, 10.0f, 2), std::vector{0.0, 10.0}); + // num == 1 + expect_near_vector(linspace(11.0f, 20.0f, 1), std::vector{11.0}); + // num == 0 + expect_near_vector(linspace(30.0f, 40.0f, 0), std::vector{}); + // start == stop + expect_near_vector(linspace(2.3f, 2.3f, 3), std::vector{2.3, 2.3, 2.3}); + // start > stop + expect_near_vector( + linspace(10.0f, 5.0f, 6), std::vector{10.0, 9.0, 8.0, 7.0, 6.0, 5.0}); + } +} + +TEST(test_linspace, linspace_int) +{ + using autoware_utils::linspace; + + // general cases + { + expect_near_vector(linspace(3, 7, 5), std::vector{3.0, 4.0, 5.0, 6.0, 7.0}); + expect_near_vector(linspace(1, 2, 3), std::vector{1.0, 1.5, 2.0}); + expect_near_vector(linspace(-1, 3, 5), std::vector{-1.0, 0.0, 1.0, 2.0, 3.0}); + } + // corner cases + { + // num == 2 + expect_near_vector(linspace(0, 10, 2), std::vector{0.0, 10.0}); + // num == 1 + expect_near_vector(linspace(11, 20, 1), std::vector{11.0}); + // num == 0 + expect_near_vector(linspace(30, 40, 0), std::vector{}); + // start == stop + expect_near_vector(linspace(3, 3, 3), std::vector{3.0, 3.0, 3.0}); + // start > stop + expect_near_vector(linspace(10, 5, 6), std::vector{10.0, 9.0, 8.0, 7.0, 6.0, 5.0}); + } +} diff --git a/common/autoware_utils/test/src/math/test_unit_conversion.cpp b/common/autoware_utils/test/src/math/test_unit_conversion.cpp new file mode 100644 index 0000000000000..0b776b7dd8d24 --- /dev/null +++ b/common/autoware_utils/test/src/math/test_unit_conversion.cpp @@ -0,0 +1,65 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/math/unit_conversion.hpp" + +#include + +using autoware_utils::pi; + +TEST(unit_conversion, deg2rad) +{ + using autoware_utils::deg2rad; + + EXPECT_DOUBLE_EQ(deg2rad(-720), -4 * pi); + EXPECT_DOUBLE_EQ(deg2rad(0), 0); + EXPECT_DOUBLE_EQ(deg2rad(30), pi / 6); + EXPECT_DOUBLE_EQ(deg2rad(60), pi / 3); + EXPECT_DOUBLE_EQ(deg2rad(90), pi / 2); + EXPECT_DOUBLE_EQ(deg2rad(180), pi); + EXPECT_DOUBLE_EQ(deg2rad(360), 2 * pi); +} + +TEST(unit_conversion, rad2deg) +{ + using autoware_utils::rad2deg; + + EXPECT_DOUBLE_EQ(rad2deg(-4 * pi), -720); + EXPECT_DOUBLE_EQ(rad2deg(0), 0); + EXPECT_DOUBLE_EQ(rad2deg(pi / 6), 30); + EXPECT_DOUBLE_EQ(rad2deg(pi / 3), 60); + EXPECT_DOUBLE_EQ(rad2deg(pi / 2), 90); + EXPECT_DOUBLE_EQ(rad2deg(pi), 180); + EXPECT_DOUBLE_EQ(rad2deg(2 * pi), 360); +} + +TEST(unit_conversion, kmph2mps) +{ + using autoware_utils::kmph2mps; + + EXPECT_DOUBLE_EQ(kmph2mps(0), 0); + EXPECT_DOUBLE_EQ(kmph2mps(36), 10); + EXPECT_DOUBLE_EQ(kmph2mps(72), 20); + EXPECT_DOUBLE_EQ(kmph2mps(180), 50); +} + +TEST(unit_conversion, mps2kmph) +{ + using autoware_utils::mps2kmph; + + EXPECT_DOUBLE_EQ(mps2kmph(0), 0); + EXPECT_DOUBLE_EQ(mps2kmph(10), 36); + EXPECT_DOUBLE_EQ(mps2kmph(20), 72); + EXPECT_DOUBLE_EQ(mps2kmph(50), 180); +} diff --git a/common/autoware_utils/test/src/system/test_stop_watch.cpp b/common/autoware_utils/test/src/system/test_stop_watch.cpp new file mode 100644 index 0000000000000..45a1c357a5b09 --- /dev/null +++ b/common/autoware_utils/test/src/system/test_stop_watch.cpp @@ -0,0 +1,62 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/system/stop_watch.hpp" + +#include + +#include +#include + +TEST(system, StopWatch_sec) +{ + using autoware_utils::StopWatch; + + StopWatch stop_watch; + + stop_watch.tic("total"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + stop_watch.tic(); + std::this_thread::sleep_for(std::chrono::seconds(1)); + const auto t1 = stop_watch.toc(); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + const auto t2 = stop_watch.toc(true); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + const auto t3 = stop_watch.toc(); + + const auto t4 = stop_watch.toc("total"); + + constexpr double error_sec = 0.01; + EXPECT_NEAR(t1, 1.0, error_sec); + EXPECT_NEAR(t2, 2.0, error_sec); + EXPECT_NEAR(t3, 1.0, error_sec); + EXPECT_NEAR(t4, 4.0, error_sec); + ASSERT_ANY_THROW(stop_watch.toc("invalid_key")); +} + +TEST(system, StopWatch_msec) +{ + using autoware_utils::StopWatch; + + StopWatch stop_watch; + + stop_watch.tic(); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + constexpr double error_msec = 10.0; + EXPECT_NEAR(stop_watch.toc(), 1000.0, error_msec); +} diff --git a/common/autoware_utils/test/src/test_autoware_utils.cpp b/common/autoware_utils/test/src/test_autoware_utils.cpp new file mode 100644 index 0000000000000..764089a855439 --- /dev/null +++ b/common/autoware_utils/test/src/test_autoware_utils.cpp @@ -0,0 +1,23 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/autoware_utils.hpp" + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_utils/test/src/trajectory/test_trajectory.cpp new file mode 100644 index 0000000000000..6aa016b26e276 --- /dev/null +++ b/common/autoware_utils/test/src/trajectory/test_trajectory.cpp @@ -0,0 +1,642 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/trajectory/tmp_conversion.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" + +#include +#include + +#include +#include + +namespace +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using TrajectoryPointArray = std::vector; +using autoware_utils::createPoint; +using autoware_utils::createQuaternionFromRPY; +using autoware_utils::transformPoint; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel; + traj.points.push_back(p); + } + + return traj; +} + +TrajectoryPointArray generateTestTrajectoryPointArray( + const size_t num_points, const double point_interval, const double vel = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + TrajectoryPointArray traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + TrajectoryPoint p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel; + traj.push_back(p); + } + + return traj; +} + +template +void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) +{ + points.at(idx).longitudinal_velocity_mps = vel; +} +} // namespace + +TEST(trajectory, validateNonEmpty) +{ + using autoware_utils::validateNonEmpty; + + // Empty + EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); + + // Non-empty + const auto traj = generateTestTrajectory(10, 1.0); + EXPECT_NO_THROW(validateNonEmpty(traj.points)); +} + +TEST(trajectory, searchZeroVelocityIndex) +{ + using autoware_utils::searchZeroVelocityIndex; + + // Empty + EXPECT_THROW(searchZeroVelocityIndex(Trajectory{}.points), std::invalid_argument); + + // No zero velocity point + { + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + EXPECT_FALSE(searchZeroVelocityIndex(traj.points)); + } + + // Only start point is zero + { + const size_t idx_ans = 0; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Only end point is zero + { + const size_t idx_ans = 9; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Only middle point is zero + { + const size_t idx_ans = 5; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Two points are zero + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + updateTrajectoryVelocityAt(traj.points, 6, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Negative velocity point is before zero velocity point + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, 2, -1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Search from src_idx to dst_idx + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 0, 3)); + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0, 4), idx_ans); + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 3, 10), idx_ans); + EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 4, 10)); + } +} + +TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) +{ + using autoware_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW( + findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Point{}), std::invalid_argument); + + // Start point + EXPECT_EQ(findNearestIndex(traj.points, createPoint(0.0, 0.0, 0.0)), 0U); + + // End point + EXPECT_EQ(findNearestIndex(traj.points, createPoint(9.0, 0.0, 0.0)), 9U); + + // Boundary conditions + EXPECT_EQ(findNearestIndex(traj.points, createPoint(0.5, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestIndex(traj.points, createPoint(0.51, 0.0, 0.0)), 1U); + + // Point before start point + EXPECT_EQ(findNearestIndex(traj.points, createPoint(-4.0, 5.0, 0.0)), 0U); + + // Point after end point + EXPECT_EQ(findNearestIndex(traj.points, createPoint(100.0, -3.0, 0.0)), 9U); + + // Random cases + EXPECT_EQ(findNearestIndex(traj.points, createPoint(2.4, 1.3, 0.0)), 2U); + EXPECT_EQ(findNearestIndex(traj.points, createPoint(4.0, 0.0, 0.0)), 4U); +} + +TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) +{ + using autoware_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); + + // Random cases + EXPECT_EQ(findNearestIndex(traj.points, createPoint(5.1, 3.4, 0.0)), 6U); +} + +TEST(trajectory, findNearestIndex_Pose_NoThreshold) +{ + using autoware_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW( + findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Pose{}, {}), std::invalid_argument); + + // Start point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); + + // End point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 9U); + + // Boundary conditions + EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); + EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.51, 0.0, 0.0, 0.0, 0.0, 0.0)), 1U); + + // Point before start point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(-4.0, 5.0, 0.0, 0.0, 0.0, 0.0)), 0U); + + // Point after end point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(100.0, -3.0, 0.0, 0.0, 0.0, 0.0)), 9U); +} + +TEST(trajectory, findNearestIndex_Pose_DistThreshold) +{ + using autoware_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Out of threshold + EXPECT_FALSE(findNearestIndex(traj.points, createPose(3.0, 0.6, 0.0, 0.0, 0.0, 0.0), 0.5)); + + // On threshold + EXPECT_EQ(*findNearestIndex(traj.points, createPose(3.0, 0.5, 0.0, 0.0, 0.0, 0.0), 0.5), 3U); + + // Within threshold + EXPECT_EQ(*findNearestIndex(traj.points, createPose(3.0, 0.4, 0.0, 0.0, 0.0, 0.0), 0.5), 3U); +} + +TEST(trajectory, findNearestIndex_Pose_YawThreshold) +{ + using autoware_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto max_d = std::numeric_limits::max(); + + // Out of threshold + EXPECT_FALSE(findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 1.1), max_d, 1.0)); + + // On threshold + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 1.0), max_d, 1.0), 3U); + + // Within threshold + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.9), max_d, 1.0), 3U); +} + +TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) +{ + using autoware_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Random cases + EXPECT_EQ(*findNearestIndex(traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4), 2U); + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0), 4U); + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1), 8U); +} + +TEST(trajectory, findNearestSegmentIndex) +{ + using autoware_utils::findNearestSegmentIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW( + findNearestSegmentIndex(Trajectory{}.points, geometry_msgs::msg::Point{}), + std::invalid_argument); + + // Start point + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(0.0, 0.0, 0.0)), 0U); + + // End point + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(9.0, 0.0, 0.0)), 8U); + + // Boundary conditions + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(1.0, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(1.1, 0.0, 0.0)), 1U); + + // Point before start point + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(-4.0, 5.0, 0.0)), 0U); + + // Point after end point + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(100.0, -3.0, 0.0)), 8U); + + // Random cases + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(2.4, 1.0, 0.0)), 2U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(4.0, 0.0, 0.0)), 3U); + + // Two nearest trajectory points are not the edges of the nearest segment. + std::vector sparse_points{ + createPoint(0.0, 0.0, 0.0), + createPoint(10.0, 0.0, 0.0), + createPoint(11.0, 0.0, 0.0), + }; + EXPECT_EQ(findNearestSegmentIndex(sparse_points, createPoint(9.0, 1.0, 0.0)), 0U); +} + +TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) +{ + using autoware_utils::calcLongitudinalOffsetToSegment; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW( + calcLongitudinalOffsetToSegment(Trajectory{}.points, {}, geometry_msgs::msg::Point{}), + std::invalid_argument); + + // Out of range + EXPECT_THROW( + calcLongitudinalOffsetToSegment(traj.points, -1, geometry_msgs::msg::Point{}), + std::out_of_range); + EXPECT_THROW( + calcLongitudinalOffsetToSegment( + traj.points, traj.points.size() - 1, geometry_msgs::msg::Point{}), + std::out_of_range); + + // Same close points in trajectory + { + const auto invalid_traj = generateTestTrajectory(10, 0.0); + const auto p = createPoint(3.0, 0.0, 0.0); + EXPECT_THROW(calcLongitudinalOffsetToSegment(invalid_traj.points, 3, p), std::runtime_error); + } + + // Same point + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 3, createPoint(3.0, 0.0, 0.0)), 0.0, epsilon); + + // Point before start point + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 6, createPoint(-3.9, 3.0, 0.0)), -9.9, epsilon); + + // Point after start point + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 7, createPoint(13.3, -10.0, 0.0)), 6.3, epsilon); + + // Random cases + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 2, createPoint(4.3, 7.0, 0.0)), 2.3, epsilon); + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 8, createPoint(1.0, 3.0, 0.0)), -7, epsilon); +} + +TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) +{ + using autoware_utils::calcLongitudinalOffsetToSegment; + + const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); + + // Random cases + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 2, createPoint(2.0, 0.5, 0.0)), 0.083861449, + epsilon); +} + +TEST(trajectory, calcLateralOffset) +{ + using autoware_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW( + calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}), std::invalid_argument); + + // Trajectory size is 1 + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + EXPECT_THROW( + calcLateralOffset(one_point_traj.points, geometry_msgs::msg::Point{}), std::out_of_range); + } + + // Same close points in trajectory + { + const auto invalid_traj = generateTestTrajectory(10, 0.0); + const auto p = createPoint(3.0, 0.0, 0.0); + EXPECT_THROW(calcLateralOffset(invalid_traj.points, p), std::runtime_error); + } + + // Point on trajectory + EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0)), 0.0, epsilon); + + // Point before start point + EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0)), 3.0, epsilon); + + // Point after start point + EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0)), -10.0, epsilon); + + // Random cases + EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0)), 7.0, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0)), -3.0, epsilon); +} + +TEST(trajectory, calcLateralOffset_CurveTrajectory) +{ + using autoware_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); + + // Random cases + EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(2.0, 0.5, 0.0)), 0.071386083, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(5.0, 1.0, 0.0)), -1.366602819, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromIndexToIndex) +{ + using autoware_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + + // Out of range + EXPECT_THROW(calcSignedArcLength(traj.points, -1, 1), std::out_of_range); + EXPECT_THROW(calcSignedArcLength(traj.points, 0, traj.points.size() + 1), std::out_of_range); + + // Same point + EXPECT_NEAR(calcSignedArcLength(traj.points, 3, 3), 0, epsilon); + + // Forward + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, 3), 3, epsilon); + + // Backward + EXPECT_NEAR(calcSignedArcLength(traj.points, 9, 5), -4, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromPointToIndex) +{ + using autoware_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + + // Same point + EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(3.0, 0.0, 0.0), 3), 0, epsilon); + + // Forward + EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(0.0, 0.0, 0.0), 3), 3, epsilon); + + // Backward + EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(9.0, 0.0, 0.0), 5), -4, epsilon); + + // Point before start point + EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(-3.9, 3.0, 0.0), 6), 9.9, epsilon); + + // Point after end point + EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(13.3, -10.0, 0.0), 7), -6.3, epsilon); + + // Random cases + EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(1.0, 3.0, 0.0), 9), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(4.3, 7.0, 0.0), 2), -2.3, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromIndexToPoint) +{ + using autoware_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + + // Same point + EXPECT_NEAR(calcSignedArcLength(traj.points, 3, createPoint(3.0, 0.0, 0.0)), 0, epsilon); + + // Forward + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, createPoint(3.0, 0.0, 0.0)), 3, epsilon); + + // Backward + EXPECT_NEAR(calcSignedArcLength(traj.points, 9, createPoint(5.0, 0.0, 0.0)), -4, epsilon); + + // Point before start point + EXPECT_NEAR(calcSignedArcLength(traj.points, 6, createPoint(-3.9, 3.0, 0.0)), -9.9, epsilon); + + // Point after end point + EXPECT_NEAR(calcSignedArcLength(traj.points, 7, createPoint(13.3, -10.0, 0.0)), 6.3, epsilon); + + // Random cases + EXPECT_NEAR(calcSignedArcLength(traj.points, 1, createPoint(9.0, 3.0, 0.0)), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 4, createPoint(1.7, 7.0, 0.0)), -2.3, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromPointToPoint) +{ + using autoware_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + + // Same point + { + const auto p = createPoint(3.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p, p), 0, epsilon); + } + + // Forward + { + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(3.0, 1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 3, epsilon); + } + + // Backward + { + const auto p1 = createPoint(8.0, 0.0, 0.0); + const auto p2 = createPoint(9.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 1, epsilon); + } + + // Point before start point + { + const auto p1 = createPoint(-3.9, 3.0, 0.0); + const auto p2 = createPoint(6.0, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 9.9, epsilon); + } + + // Point after end point + { + const auto p1 = createPoint(7.0, -5.0, 0.0); + const auto p2 = createPoint(13.3, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 6.3, epsilon); + } + + // Point before start point and after end point + { + const auto p1 = createPoint(-4.3, 10.0, 0.0); + const auto p2 = createPoint(13.8, -1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 18.1, epsilon); + } + + // Random cases + { + const auto p1 = createPoint(1.0, 3.0, 0.0); + const auto p2 = createPoint(9.0, -1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 8, epsilon); + } + { + const auto p1 = createPoint(4.3, 7.0, 0.0); + const auto p2 = createPoint(2.0, 3.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), -2.3, epsilon); + } +} + +TEST(trajectory, calcArcLength) +{ + using autoware_utils::calcArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW(calcArcLength(Trajectory{}.points), std::invalid_argument); + + // Whole Length + EXPECT_NEAR(calcArcLength(traj.points), 9.0, epsilon); +} + +TEST(trajectory, convertToTrajectory) +{ + using autoware_utils::convertToTrajectory; + + // Size check + { + const auto traj_input = generateTestTrajectoryPointArray(50, 1.0); + const auto traj = convertToTrajectory(traj_input); + EXPECT_EQ(traj.points.size(), traj_input.size()); + } + + // Clipping check + { + const auto traj_input = generateTestTrajectoryPointArray(10000, 1.0); + const auto traj = convertToTrajectory(traj_input); + EXPECT_EQ(traj.points.size(), traj.CAPACITY); + // Value check + for (size_t i = 0; i < traj.points.size(); ++i) { + EXPECT_EQ(traj.points.at(i), traj_input.at(i)); + } + } +} + +TEST(trajectory, convertToTrajectoryPointArray) +{ + using autoware_utils::convertToTrajectoryPointArray; + + const auto traj_input = generateTestTrajectory(100, 1.0); + const auto traj = convertToTrajectoryPointArray(traj_input); + + // Size check + EXPECT_EQ(traj.size(), traj_input.points.size()); + + // Value check + for (size_t i = 0; i < traj.size(); ++i) { + EXPECT_EQ(traj.at(i), traj_input.points.at(i)); + } +}