diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp index 516b3ab09e8b7..8c1d49fb5f607 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -39,6 +39,10 @@ std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); + +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio); } // namespace interpolation #endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index c3595d212f349..e44626498a80b 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -57,4 +57,15 @@ std::vector slerp( return query_values; } +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} } // namespace interpolation diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 6e77c1ec4a186..7d4be216e89fe 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" @@ -23,6 +25,8 @@ namespace motion_utils { +using TrajectoryPoints = std::vector; + /** * @brief Convert std::vector to * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to @@ -45,6 +49,73 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( std::vector convertToTrajectoryPointArray( const autoware_auto_planning_msgs::msg::Trajectory & trajectory); +template +autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); + throw std::logic_error("Only specializations of convertToPath can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::Path convertToPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + autoware_auto_planning_msgs::msg::Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + +template +TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); + throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); +} + +template <> +inline TrajectoryPoints convertToTrajectoryPoints( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + TrajectoryPoints output{}; + for (const auto & p : input.points) { + autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + output.emplace_back(tp); + } + return output; +} + +template +autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + [[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); + throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + const TrajectoryPoints & input) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + for (const auto & p : input) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + output.points.emplace_back(pp); + } + return output; +} + } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 9c01f7bc26c4b..cc1c9c08887ba 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -16,6 +16,8 @@ #define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" @@ -74,14 +76,6 @@ Pose calcPoseAfterTimeDelay( const Pose & current_pose, const double delay_time, const double current_vel, const double current_acc); -/** - * @brief apply linear interpolation to orientation - * @param [in] o_from first orientation - * @param [in] o_to second orientation - * @param [in] ratio ratio between o_from and o_to for interpolation - */ -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - /** * @brief apply linear interpolation to trajectory point that is nearest to a certain point * @param [in] points trajectory points @@ -107,7 +101,7 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); interpolated_point.pose.position.y = interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.orientation = lerpOrientation( + interpolated_point.pose.orientation = interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); interpolated_point.longitudinal_velocity_mps = interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9791a1f0b5e3e..33a54663ccaef 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio) return v_from + (v_to - v_from) * ratio; } -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val) diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 5c7698180f82b..58aa867b2deab 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" @@ -303,7 +305,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +313,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +322,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +330,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +338,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 12715520e1146..9b3fbedc37203 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include #include @@ -700,7 +701,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = utils::toPath(*path_candidate_ptr); + output = motion_utils::convertToPath( + *path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp index 4ac6f01da65fb..f109bb2cbb213 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp @@ -58,8 +58,6 @@ PathWithLaneId resamplePathWithSpline( const PathWithLaneId & path, const double interval, const bool keep_input_points = false, const std::pair target_section = {0.0, std::numeric_limits::max()}); -Path toPath(const PathWithLaneId & input); - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc); diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index 2eceb0ffb81c8..1d51426793de8 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -155,19 +155,6 @@ PathWithLaneId resamplePathWithSpline( return motion_utils::resamplePath(path, s_out); } -Path toPath(const PathWithLaneId & input) -{ - Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc) { diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index c86272cff1c2e..2184be4fbd2fe 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -35,11 +35,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path); -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory); - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 7fe8612cc6858..314281a954b49 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. // #include +#include "motion_utils/trajectory/conversion.hpp" + #include #include #include @@ -41,41 +43,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) -{ - TrajectoryPoints tps; - for (const auto & p : path.points) { - TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - tps.emplace_back(tp); - } - return tps; -} -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) -{ - PathWithLaneId path; - for (const auto & p : trajectory) { - PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - path.points.emplace_back(pp); - } - return path; -} - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, @@ -87,7 +54,9 @@ bool smoothPath( const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; - auto trajectory = convertPathToTrajectoryPoints(in_path); + auto trajectory = + motion_utils::convertToTrajectoryPoints( + in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = @@ -117,7 +86,7 @@ bool smoothPath( motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } - out_path = convertTrajectoryPointsToPath(traj_smoothed); + out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp index 4d8d7be96b44b..371a6316ce993 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp @@ -492,19 +492,6 @@ void updateNodeOptions( node_options.arguments(std::vector{arguments.begin(), arguments.end()}); } -Path toPath(const PathWithLaneId & input) -{ - Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - PathWithLaneId loadPathWithLaneIdInYaml() { const auto planning_test_utils_dir = diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 23bfb2f44cb96..278813818de0d 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -23,6 +23,7 @@ component_interface_utils lanelet2_extension lanelet2_io + motion_utils nav_msgs rclcpp route_handler diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index a9d096bf00fd4..0a6b4246348eb 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/trajectory/conversion.hpp" + #include #include @@ -289,7 +291,9 @@ void PlanningInterfaceTestManager::publishNominalPath( { test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - test_utils::toPath(test_utils::loadPathWithLaneIdInYaml()), 5); + motion_utils::convertToPath( + test_utils::loadPathWithLaneIdInYaml()), + 5); } void PlanningInterfaceTestManager::publishAbnormalPath(