diff --git a/planning/collision_free_path_planner/CMakeLists.txt b/planning/collision_free_path_planner/CMakeLists.txt index 57303888e0848..b97a56d42be8b 100644 --- a/planning/collision_free_path_planner/CMakeLists.txt +++ b/planning/collision_free_path_planner/CMakeLists.txt @@ -8,8 +8,9 @@ find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) ament_auto_add_library(collision_free_path_planner SHARED - # node and core algorithms + # node src/node.cpp + # core algorithms src/replan_checker.cpp src/costmap_generator.cpp src/eb_path_optimizer.cpp @@ -20,7 +21,8 @@ ament_auto_add_library(collision_free_path_planner SHARED src/vehicle_model/vehicle_model_interface.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp # utils - src/utils/utils.cpp + src/utils/trajectory_utils.cpp + src/utils/geometry_utils.cpp src/utils/cv_utils.cpp ) diff --git a/planning/collision_free_path_planner/README.md b/planning/collision_free_path_planner/README.md index c4912eecb1ea3..9da6f002e121e 100644 --- a/planning/collision_free_path_planner/README.md +++ b/planning/collision_free_path_planner/README.md @@ -39,14 +39,14 @@ start :createPlannerData; -group generateOptimizedTrajectory - group getMaps - :getDrivableArea; - :getRoadClearanceMap; - :drawObstacleOnImage; - :getObstacleClearanceMap; - end group +group getMaps + :getDrivableArea; + :getRoadClearanceMap; + :drawObstacleOnImage; + :getObstacleClearanceMap; +end group +group generateOptimizedTrajectory group optimizeTrajectory :checkReplan; if (replanning required?) then (yes) @@ -61,16 +61,15 @@ group generateOptimizedTrajectory endif end group - :updateVelocity; + :applyPathVelocity; :insertZeroVelocityOutsideDrivableArea; :publishDebugMarkerInOptimization; end group -:extendedOptimizedTrajectory; -:alignVelocity; +:extendTrajectory; -:convertToTrajectory; +:setZeroVelocityAfterStopPoint; :publishDebugDataInMain; diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/common_structs.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/common_structs.hpp index 9f84a5b006549..9611c113f4cb5 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/common_structs.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/common_structs.hpp @@ -128,6 +128,7 @@ struct DebugData { msg_stream = StreamWithPrint{}; msg_stream.enable_calculation_time_info = enable_calculation_time_info; + stop_pose_by_drivable_area = boost::none; } // string stream for calculation time diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/debug_marker.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/debug_marker.hpp index f94448a0c00fb..af0cc9fb2ecfa 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/debug_marker.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/debug_marker.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/eb_path_optimizer.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/eb_path_optimizer.hpp index e86e976fe5354..47e364a92a162 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/eb_path_optimizer.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/eb_path_optimizer.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/mpt_optimizer.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/mpt_optimizer.hpp index 74e32a22764ab..c93a55c70f139 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/mpt_optimizer.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/mpt_optimizer.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -16,7 +16,7 @@ * * MIT License * - * Copyright (c) 2020 Li Jiangnan + * Copyright (c) 2022 Li Jiangnan * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp index c73ee41bfd35d..ac289d5f7daac 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -222,20 +222,19 @@ class CollisionFreePathPlanner : public rclcpp::Node // main functions bool isDataReady(); PlannerData createPlannerData(const Path & path); - std::vector generateOptimizedTrajectory(const PlannerData & planner_data); + std::vector generateOptimizedTrajectory( + const PlannerData & planner_data, const CVMaps & cv_maps); std::vector extendTrajectory( const std::vector & path_points, const std::vector & optimized_points); - std::vector generatePostProcessedTrajectory( - const PlannerData & planner_data, const std::vector & merged_optimized_points); void publishDebugDataInMain(const Path & path) const; - // functions for optimization + // functions in generateOptimizedTrajectory std::vector optimizeTrajectory( const PlannerData & planner_data, const CVMaps & cv_maps); std::vector getPrevOptimizedTrajectory( const std::vector & path_points) const; - void updateVelocity( + void applyPathVelocity( std::vector & traj_points, const std::vector & path_points) const; void insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & traj_points, diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/replan_checker.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/replan_checker.hpp index 666a36833a3ab..f61336a375c11 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/replan_checker.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/replan_checker.hpp @@ -18,7 +18,6 @@ #include "collision_free_path_planner/common_structs.hpp" #include "collision_free_path_planner/mpt_optimizer.hpp" #include "collision_free_path_planner/type_alias.hpp" -#include "collision_free_path_planner/utils/utils.hpp" #include diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/utils/cv_utils.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/cv_utils.hpp index 6e60b69fec9af..95196e4159c5c 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/utils/cv_utils.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/cv_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/utils/geometry_utils.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/geometry_utils.hpp new file mode 100644 index 0000000000000..d95fa30050084 --- /dev/null +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/geometry_utils.hpp @@ -0,0 +1,108 @@ +// Copyright 2022 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 COLLISION_FREE_PATH_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ +#define COLLISION_FREE_PATH_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ + +#include "collision_free_path_planner/common_structs.hpp" +#include "collision_free_path_planner/type_alias.hpp" +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include +#include +#include +#include + +struct collision_free_path_planner::ReferencePoint; + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const collision_free_path_planner::ReferencePoint & p); + +template <> +geometry_msgs::msg::Pose getPose(const collision_free_path_planner::ReferencePoint & p); +} // namespace tier4_autoware_utils + +namespace collision_free_path_planner +{ +namespace geometry_utils +{ +template +geometry_msgs::msg::Point transformToRelativeCoordinate2D( + const T & point, const geometry_msgs::msg::Pose & origin) +{ + // NOTE: implement transformation without defining yaw variable + // but directly sin/cos of yaw for fast calculation + const auto & q = origin.orientation; + const double cos_yaw = 1 - 2 * q.z * q.z; + const double sin_yaw = 2 * q.w * q.z; + + geometry_msgs::msg::Point relative_p; + const double tmp_x = point.x - origin.position.x; + const double tmp_y = point.y - origin.position.y; + relative_p.x = tmp_x * cos_yaw + tmp_y * sin_yaw; + relative_p.y = -tmp_x * sin_yaw + tmp_y * cos_yaw; + relative_p.z = point.z; + + return relative_p; +} + +geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin); + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); + +template +geometry_msgs::msg::Point transformMapToImage( + const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info) +{ + geometry_msgs::msg::Point relative_p = + transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); + double resolution = occupancy_grid_info.resolution; + double map_y_height = occupancy_grid_info.height; + double map_x_width = occupancy_grid_info.width; + double map_x_in_image_resolution = relative_p.x / resolution; + double map_y_in_image_resolution = relative_p.y / resolution; + geometry_msgs::msg::Point image_point; + image_point.x = map_y_height - map_y_in_image_resolution; + image_point.y = map_x_width - map_x_in_image_resolution; + return image_point; +} + +boost::optional transformMapToOptionalImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info); + +bool transformMapToImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point); +} // namespace geometry_utils +} // namespace collision_free_path_planner +#endif // COLLISION_FREE_PATH_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/trajectory_utils.hpp similarity index 71% rename from planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp rename to planning/collision_free_path_planner/include/collision_free_path_planner/utils/trajectory_utils.hpp index 2fe3f8a6a7c37..9e62d16e98c9c 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/trajectory_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COLLISION_FREE_PATH_PLANNER__UTILS__UTILS_HPP_ -#define COLLISION_FREE_PATH_PLANNER__UTILS__UTILS_HPP_ +#ifndef COLLISION_FREE_PATH_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ +#define COLLISION_FREE_PATH_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ #include "collision_free_path_planner/common_structs.hpp" #include "collision_free_path_planner/type_alias.hpp" @@ -34,8 +34,6 @@ #include #include -struct collision_free_path_planner::ReferencePoint; - namespace tier4_autoware_utils { template <> @@ -47,128 +45,55 @@ geometry_msgs::msg::Pose getPose(const collision_free_path_planner::ReferencePoi namespace collision_free_path_planner { -namespace geometry_utils +namespace trajectory_utils { template -geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const T & point, const geometry_msgs::msg::Pose & origin) +size_t findForwardIndex(const T & points, const size_t begin_idx, const double forward_length) { - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point relative_p; - const double tmp_x = point.x - origin.position.x; - const double tmp_y = point.y - origin.position.y; - relative_p.x = tmp_x * cos_yaw + tmp_y * sin_yaw; - relative_p.y = -tmp_x * sin_yaw + tmp_y * cos_yaw; - relative_p.z = point.z; - - return relative_p; + double sum_length = 0.0; + for (size_t i = begin_idx; i < points.size() - 1; ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + if (sum_length > forward_length) { + return i; + } + } + return points.size() - 1; } -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); - template -geometry_msgs::msg::Point transformMapToImage( - const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - double resolution = occupancy_grid_info.resolution; - double map_y_height = occupancy_grid_info.height; - double map_x_width = occupancy_grid_info.width; - double map_x_in_image_resolution = relative_p.x / resolution; - double map_y_in_image_resolution = relative_p.y / resolution; - geometry_msgs::msg::Point image_point; - image_point.x = map_y_height - map_y_in_image_resolution; - image_point.y = map_x_width - map_x_in_image_resolution; - return image_point; -} - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info); - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point); -} // namespace geometry_utils - -/* -namespace interpolation_utils +T cropPointsAfterOffsetPoint( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double offset) { -std::vector interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double offset); - -std::vector interpolateConnected2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double begin_yaw, const double end_yaw); - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, - const std::vector & base_yaw, const double resolution); + if (points.empty()) { + return T{}; + } -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution); + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); -template -std::vector getInterpolatedPoints( - const T & points, const double delta_arc_length, const double offset = 0) -{ - constexpr double epsilon = 1e-6; - std::vector tmp_x; - std::vector tmp_y; - for (size_t i = 0; i < points.size(); i++) { - const auto & current_point = tier4_autoware_utils::getPoint(points.at(i)); - if (i > 0) { - const auto & prev_point = tier4_autoware_utils::getPoint(points.at(i - 1)); - if ( - std::fabs(current_point.x - prev_point.x) < epsilon && - std::fabs(current_point.y - prev_point.y) < epsilon) { - continue; + // search forward + if (sum_length < offset) { + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (offset < sum_length) { + return T{points.begin() + i - 1, points.end()}; } } - tmp_x.push_back(current_point.x); - tmp_y.push_back(current_point.y); - } - - return interpolation_utils::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, offset); -} - -std::vector getInterpolatedTrajectoryPoints( - const std::vector & points, const double delta_arc_length); -std::vector getConnectedInterpolatedPoints( - const std::vector & points, const double delta_arc_length, - const double begin_yaw, const double end_yaw); -} // namespace interpolation_utils -*/ + return T{}; + } -namespace points_utils -{ -template -size_t findForwardIndex(const T & points, const size_t begin_idx, const double forward_length) -{ - double sum_length = 0.0; - for (size_t i = begin_idx; i < points.size() - 1; ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - if (sum_length > forward_length) { - return i; + // search backward + for (size_t i = target_seg_idx; 0 < i; + --i) { // NOTE: use size_t since i is always positive value + sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (sum_length < offset) { + return T{points.begin() + i - 1, points.end()}; } } - return points.size() - 1; + + return points; } template @@ -182,6 +107,7 @@ T cropForwardPoints( double sum_length = -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + std::cerr << "sum length " << sum_length << std::endl; for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length) { @@ -462,6 +388,6 @@ Trajectory createTrajectory( std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval); -} // namespace points_utils +} // namespace trajectory_utils } // namespace collision_free_path_planner -#endif // COLLISION_FREE_PATH_PLANNER__UTILS__UTILS_HPP_ +#endif // COLLISION_FREE_PATH_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/collision_free_path_planner/scripts/trajectory_visualizer.py b/planning/collision_free_path_planner/scripts/trajectory_visualizer.py deleted file mode 100755 index 40cb7ea854ec7..0000000000000 --- a/planning/collision_free_path_planner/scripts/trajectory_visualizer.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -# TODO(kosuke murakami): write ros2 visualizer -# import rospy -# from tier4_planning_msgs.msg import Trajectory -# from tier4_planning_msgs.msg import TrajectoryPoint -# import matplotlib.pyplot as plt -# import numpy as np -# import tf -# from geometry_msgs.msg import Vector3 - - -# def quaternion_to_euler(quaternion): -# """Convert Quaternion to Euler Angles - -# quaternion: geometry_msgs/Quaternion -# euler: geometry_msgs/Vector3 -# """ -# e = tf.transformations.euler_from_quaternion( -# (quaternion.x, quaternion.y, quaternion.z, quaternion.w)) -# return Vector3(x=e[0], y=e[1], z=e[2]) - - -# class TrajectoryVisualizer(): - -# def __init__(self): -# self.in_trajectory = Trajectory() -# self.debug_trajectory = Trajectory() -# self.debug_fixed_trajectory = Trajectory() - -# self.plot_done1 = True -# self.plot_done2 = True -# self.plot_done3 = True - -# self.length = 50 - -# self.sub_status1 = rospy.Subscriber( -# "/planning/scenario_planning/lane_driving/motion_planning/collision_free_path_planner/trajectory", -# Trajectory, self.CallBackTraj, queue_size=1, tcp_nodelay=True) -# rospy.Timer(rospy.Duration(0.3), self.timerCallback) - -# def CallBackTraj(self, cmd): -# if (self.plot_done1): -# self.in_trajectory = cmd -# self.plot_done1 = False - -# def CallBackDebugTraj(self, cmd): -# if (self.plot_done2): -# self.debug_trajectory = cmd -# self.plot_done2 = False - -# def CallBackDebugFixedTraj(self, cmd): -# if (self.plot_done3): -# self.debug_fixed_trajectory = cmd -# self.plot_done3 = False - -# def timerCallback(self, event): -# self.plotTrajectory() -# self.plot_done1 = True -# self.plot_done2 = True -# self.plot_done3 = True - -# def CalcArcLength(self, traj): -# s_arr = [] -# ds = 0.0 -# s_sum = 0.0 - -# if len(traj.points) > 0: -# s_arr.append(s_sum) - -# for i in range(1, len(traj.points)): -# p0 = traj.points[i-1] -# p1 = traj.points[i] -# dx = p1.pose.position.x - p0.pose.position.x -# dy = p1.pose.position.y - p0.pose.position.y -# ds = np.sqrt(dx**2 + dy**2) -# s_sum += ds -# if(s_sum > self.length): -# break -# s_arr.append(s_sum) -# return s_arr - -# def CalcX(self, traj): -# v_list = [] -# for p in traj.points: -# v_list.append(p.pose.position.x) -# return v_list - -# def CalcY(self, traj): -# v_list = [] -# for p in traj.points: -# v_list.append(p.pose.position.y) -# return v_list - -# def CalcYaw(self, traj, s_arr): -# v_list = [] -# for p in traj.points: -# v_list.append(quaternion_to_euler(p.pose.orientation).z) -# return v_list[0: len(s_arr)] - -# def plotTrajectory(self): -# plt.clf() -# ax3 = plt.subplot(1, 1, 1) -# x = self.CalcArcLength(self.in_trajectory) -# y = self.CalcYaw(self.in_trajectory, x) -# if len(x) == len(y): -# ax3.plot(x, y, label="final", marker="*") -# ax3.set_xlabel("arclength [m]") -# ax3.set_ylabel("yaw") -# plt.pause(0.01) - - -# def main(): -# rospy.init_node("trajectory_visualizer") -# TrajectoryVisualizer() -# rospy.spin() - - -# if __name__ == "__main__": -# main() diff --git a/planning/collision_free_path_planner/src/costmap_generator.cpp b/planning/collision_free_path_planner/src/costmap_generator.cpp index ff79cb32bfb65..9a7d7f18fa9fd 100644 --- a/planning/collision_free_path_planner/src/costmap_generator.cpp +++ b/planning/collision_free_path_planner/src/costmap_generator.cpp @@ -16,7 +16,7 @@ #include "collision_free_path_planner/debug_marker.hpp" #include "collision_free_path_planner/utils/cv_utils.hpp" -#include "collision_free_path_planner/utils/utils.hpp" +#include "collision_free_path_planner/utils/geometry_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" namespace tier4_autoware_utils diff --git a/planning/collision_free_path_planner/src/debug_marker.cpp b/planning/collision_free_path_planner/src/debug_marker.cpp index cefab4c0a5e6c..3b0118b1b8eeb 100644 --- a/planning/collision_free_path_planner/src/debug_marker.cpp +++ b/planning/collision_free_path_planner/src/debug_marker.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -17,7 +17,7 @@ #include "collision_free_path_planner/eb_path_optimizer.hpp" #include "collision_free_path_planner/mpt_optimizer.hpp" #include "collision_free_path_planner/utils/cv_utils.hpp" -#include "collision_free_path_planner/utils/utils.hpp" +// #include "collision_free_path_planner/utils/utils.hpp" #include "motion_utils/motion_utils.hpp" #include "visualization_msgs/msg/marker_array.hpp" diff --git a/planning/collision_free_path_planner/src/eb_path_optimizer.cpp b/planning/collision_free_path_planner/src/eb_path_optimizer.cpp index 35b174bd6f692..2695dbab2a049 100644 --- a/planning/collision_free_path_planner/src/eb_path_optimizer.cpp +++ b/planning/collision_free_path_planner/src/eb_path_optimizer.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -15,7 +15,8 @@ #include "collision_free_path_planner/eb_path_optimizer.hpp" #include "collision_free_path_planner/type_alias.hpp" -#include "collision_free_path_planner/utils/utils.hpp" +#include "collision_free_path_planner/utils/geometry_utils.hpp" +#include "collision_free_path_planner/utils/trajectory_utils.hpp" #include "motion_utils/motion_utils.hpp" #include @@ -230,7 +231,7 @@ EBPathOptimizer::getEBTrajectory( debug_data_ptr_->eb_traj = eb_traj_points; { // publish eb trajectory - const auto eb_traj = points_utils::createTrajectory(p.path.header, eb_traj_points); + const auto eb_traj = trajectory_utils::createTrajectory(p.path.header, eb_traj_points); debug_eb_traj_pub_->publish(eb_traj); } @@ -255,8 +256,8 @@ EBPathOptimizer::getOptimizedTrajectory( // crop path const size_t ego_seg_idx = - points_utils::findEgoSegmentIndex(path.points, ego_pose, ego_nearest_param_); - const auto cropped_path_points = points_utils::cropPoints( + trajectory_utils::findEgoSegmentIndex(path.points, ego_pose, ego_nearest_param_); + const auto cropped_path_points = trajectory_utils::cropPoints( path.points, ego_pose.position, ego_seg_idx, forward_distance + tmp_margin, -backward_distance - tmp_margin); diff --git a/planning/collision_free_path_planner/src/mpt_optimizer.cpp b/planning/collision_free_path_planner/src/mpt_optimizer.cpp index ae883eb1a5722..e3b301fdcb48a 100644 --- a/planning/collision_free_path_planner/src/mpt_optimizer.cpp +++ b/planning/collision_free_path_planner/src/mpt_optimizer.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -14,7 +14,8 @@ #include "collision_free_path_planner/mpt_optimizer.hpp" -#include "collision_free_path_planner/utils/utils.hpp" +#include "collision_free_path_planner/utils/geometry_utils.hpp" +#include "collision_free_path_planner/utils/trajectory_utils.hpp" #include "collision_free_path_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/motion_utils.hpp" @@ -216,7 +217,7 @@ double calcLateralError( return lat_err; } -Eigen::Vector2d getState( +[[maybe_unused]] Eigen::Vector2d getState( const geometry_msgs::msg::Pose & target_pose, const geometry_msgs::msg::Pose & ref_pose) { const double lat_error = calcLateralError(target_pose.position, ref_pose); @@ -249,12 +250,13 @@ std::vector createVector(const T & value, const std::vector & vector) return result_vector; } -std::vector resampleRefPoints( +[[maybe_unused]] std::vector resampleRefPoints( const std::vector & ref_points, const double interval) { - const auto traj_points = points_utils::convertToTrajectoryPoints(ref_points); - const auto resampled_traj_points = points_utils::resampleTrajectoryPoints(traj_points, interval); - return points_utils::convertToReferencePoints(resampled_traj_points); + const auto traj_points = trajectory_utils::convertToTrajectoryPoints(ref_points); + const auto resampled_traj_points = + trajectory_utils::resampleTrajectoryPoints(traj_points, interval); + return trajectory_utils::convertToReferencePoints(resampled_traj_points); } } // namespace @@ -709,14 +711,14 @@ std::vector MPTOptimizer::getReferencePoints( // convert smoothed points to reference points const auto resampled_smoothed_points = - points_utils::resampleTrajectoryPoints(smoothed_points, mpt_param_.delta_arc_length); - auto ref_points = points_utils::convertToReferencePoints(resampled_smoothed_points); + trajectory_utils::resampleTrajectoryPoints(smoothed_points, mpt_param_.delta_arc_length); + auto ref_points = trajectory_utils::convertToReferencePoints(resampled_smoothed_points); // crop reference points with margin first to reduce calculation cost const double tmp_margin = 10.0; const size_t ego_seg_idx = - points_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); - ref_points = points_utils::cropPoints( + trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); + ref_points = trajectory_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_distance + tmp_margin, -backward_distance - tmp_margin); @@ -730,7 +732,7 @@ std::vector MPTOptimizer::getReferencePoints( calcOrientation(ref_points); // crop backward - ref_points = points_utils::cropPoints( + ref_points = trajectory_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_distance + tmp_margin, -backward_distance); @@ -749,7 +751,7 @@ std::vector MPTOptimizer::getReferencePoints( /* // crop forward - ref_points = points_utils::cropForwardPoints( + ref_points = trajectory_utils::cropForwardPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_distance); */ @@ -759,7 +761,7 @@ std::vector MPTOptimizer::getReferencePoints( const auto resampled_smoothed_points = resampleTrajectoryPoints(smoothed_points, mpt_param_.delta_arc_length); const size_t ego_seg_idx = findEgoSegmentIndex(resampled_smoothed_points, p.ego_pose, ego_nearest_param_); return - points_utils::cropBackwardPoints(resampled_smoothed_points, p.ego_pose.position, ego_seg_idx, + trajectory_utils::cropBackwardPoints(resampled_smoothed_points, p.ego_pose.position, ego_seg_idx, traj_param_.output_backward_traj_length); }(); @@ -779,7 +781,7 @@ std::vector MPTOptimizer::getReferencePoints( constexpr double tmp_ref_points_margin = 20.0; const double ref_length_with_margin = traj_param_.num_sampling_points * mpt_param_.delta_arc_length + tmp_ref_points_margin; - ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length_with_margin); + ref_points = trajectory_utils::clipForwardPoints(ref_points, 0, ref_length_with_margin); if (ref_points.empty()) { return std::vector{}; } @@ -793,7 +795,7 @@ std::vector MPTOptimizer::getReferencePoints( calcExtraPoints(ref_points); const double ref_length = traj_param_.num_sampling_points * mpt_param_.delta_arc_length; - ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length); + ref_points = trajectory_utils::clipForwardPoints(ref_points, 0, ref_length); // bounds information is assigned to debug data after truncating reference points debug_data_ptr_->ref_points = ref_points; @@ -820,7 +822,7 @@ void MPTOptimizer::calcFixedPoints(std::vector & ref_points) con */ if (prev_ref_points_ptr_) { - const size_t prev_ref_front_seg_idx = points_utils::findEgoSegmentIndex( + const size_t prev_ref_front_seg_idx = trajectory_utils::findEgoSegmentIndex( *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.front()), ego_nearest_param_); const size_t prev_ref_front_point_idx = prev_ref_front_seg_idx; @@ -859,15 +861,15 @@ void MPTOptimizer::calcPlanningFromEgo( // const auto interpolated_points = interpolation_utils::getInterpolatedPoints( // points, // mpt_param_.delta_arc_length); const auto cropped_interpolated_points = - // points_utils::clipBackwardPoints( interpolated_points, current_pose_.position, + // trajectory_utils::clipBackwardPoints( interpolated_points, current_pose_.position, // traj_param_.output_backward_traj_length, mpt_param_.delta_arc_length); // // auto cropped_ref_points = - // points_utils::convertToReferencePoints(cropped_interpolated_points); + // trajectory_utils::convertToReferencePoints(cropped_interpolated_points); // assign fix kinematics const size_t nearest_ref_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)), + trajectory_utils::convertToPosesWithYawEstimation(trajectory_utils::convertToPoints(ref_points)), ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); // calculate cropped_ref_points.at(nearest_ref_idx) with yaw @@ -906,7 +908,7 @@ std::vector MPTOptimizer::getFixedReferencePoints( const auto & prev_ref_points = prev_trajs->ref_points; const int nearest_prev_ref_idx = static_cast(motion_utils::findFirstNearestIndexWithSoftConstraints( - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(prev_ref_points)), + trajectory_utils::convertToPosesWithYawEstimation(trajectory_utils::convertToPoints(prev_ref_points)), ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold)); // calculate begin_prev_ref_idx @@ -1036,7 +1038,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( const size_t D_v = D_x + (N_ref - 1) * D_u; - const bool is_containing_path_terminal_point = points_utils::isNearLastPathPoint( + const bool is_containing_path_terminal_point = trajectory_utils::isNearLastPathPoint( ref_points.back(), path_points, 0.0001, traj_param_.delta_yaw_threshold_for_closest_point); // update Q @@ -1133,7 +1135,7 @@ boost::optional MPTOptimizer::executeOptimization( ref_front_point.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.front().yaw); - const size_t seg_idx = points_utils::findSoftNearestIndex( + const size_t seg_idx = trajectory_utils::findSoftNearestIndex( *prev_ref_points_ptr_, ref_front_point, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); @@ -1647,7 +1649,7 @@ void MPTOptimizer::calcCurvature(std::vector & ref_points) const size_t max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); size_t L = std::min(static_cast(mpt_param_.num_curvature_sampling_points), max_smoothing_num); - auto curvatures = points_utils::calcCurvature( + auto curvatures = trajectory_utils::calcCurvature( ref_points, static_cast(mpt_param_.num_curvature_sampling_points)); for (size_t i = L; i < num_points - L; ++i) { if (!ref_points.at(i).fix_kinematic_state) { @@ -1679,7 +1681,7 @@ void MPTOptimizer::calcExtraPoints(std::vector & ref_points) con for (size_t i = 0; i < ref_points.size(); ++i) { // alpha const auto front_wheel_pos = - points_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); + trajectory_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); const bool are_too_close_points = tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).p) < 1e-03; @@ -1710,9 +1712,9 @@ void MPTOptimizer::calcExtraPoints(std::vector & ref_points) con // The point are considered to be near the object if nearest previous ref point is near the // object. if (prev_ref_points_ptr_ && prev_ref_points_ptr_->empty()) { - const auto ref_points_with_yaw = - points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); - const size_t prev_idx = points_utils::findSoftNearestIndex( + const auto ref_points_with_yaw = trajectory_utils::convertToPosesWithYawEstimation( + trajectory_utils::convertToPoints(ref_points)); + const size_t prev_idx = trajectory_utils::findSoftNearestIndex( *prev_ref_points_ptr_, ref_points_with_yaw.at(i), traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); @@ -1859,7 +1861,8 @@ void MPTOptimizer::calcVehicleBounds( } SplineInterpolationPoints2d ref_points_spline_interpolation; - ref_points_spline_interpolation.calcSplineCoefficients(points_utils::convertToPoints(ref_points)); + ref_points_spline_interpolation.calcSplineCoefficients( + trajectory_utils::convertToPoints(ref_points)); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -2099,14 +2102,14 @@ void MPTOptimizer::publishDebugTrajectories( const std::vector & mpt_traj_points) const { const auto mpt_fixed_traj_points = extractMPTFixedPoints(ref_points); - const auto mpt_fixed_traj = points_utils::createTrajectory(header, mpt_fixed_traj_points); + const auto mpt_fixed_traj = trajectory_utils::createTrajectory(header, mpt_fixed_traj_points); debug_mpt_fixed_traj_pub_->publish(mpt_fixed_traj); - const auto ref_traj = - points_utils::createTrajectory(header, points_utils::convertToTrajectoryPoints(ref_points)); + const auto ref_traj = trajectory_utils::createTrajectory( + header, trajectory_utils::convertToTrajectoryPoints(ref_points)); debug_mpt_ref_traj_pub_->publish(ref_traj); - const auto mpt_traj = points_utils::createTrajectory(header, mpt_traj_points); + const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points); debug_mpt_traj_pub_->publish(mpt_traj); } diff --git a/planning/collision_free_path_planner/src/node.cpp b/planning/collision_free_path_planner/src/node.cpp index 7573ba60b8caa..7051261a362f2 100644 --- a/planning/collision_free_path_planner/src/node.cpp +++ b/planning/collision_free_path_planner/src/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -16,7 +16,7 @@ #include "collision_free_path_planner/debug_marker.hpp" #include "collision_free_path_planner/utils/cv_utils.hpp" -#include "collision_free_path_planner/utils/utils.hpp" +#include "collision_free_path_planner/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" @@ -58,6 +58,16 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & msg.data = data; return msg; } + +void setZeroVelocityAfterStopPoint(std::vector & traj_points) +{ + const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); + if (opt_zero_vel_idx) { + for (size_t i = opt_zero_vel_idx.get(); i < traj_points.size(); ++i) { + traj_points.at(i).longitudinal_velocity_mps = 0.0; + } + } +} } // namespace CollisionFreePathPlanner::CollisionFreePathPlanner(const rclcpp::NodeOptions & node_options) @@ -208,8 +218,8 @@ void CollisionFreePathPlanner::onPath(const Path::SharedPtr path_ptr) if (!is_driving_forward) { logWarnThrottle(3000, "Backward path is NOT supported. Just converting path to trajectory"); - const auto traj_points = points_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = points_utils::createTrajectory(path_ptr->header, traj_points); + const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); + const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points); traj_pub_->publish(output_traj_msg); return; } @@ -217,18 +227,19 @@ void CollisionFreePathPlanner::onPath(const Path::SharedPtr path_ptr) // 1. create planner data const auto planner_data = createPlannerData(*path_ptr); - // 2. generate optimized trajectory - const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); + // 2. create clearance maps + const CVMaps cv_maps = costmap_generator_ptr_->getMaps(planner_data, traj_param_); + + // 3. generate optimized trajectory + const auto optimized_traj_points = generateOptimizedTrajectory(planner_data, cv_maps); - // 3. extend trajectory to connect the optimized trajectory and the following path smoothly - const auto extended_traj_points = - extendTrajectory(planner_data.path.points, optimized_traj_points); + // 4. extend trajectory to connect the optimized trajectory and the following path smoothly + auto extended_traj_points = extendTrajectory(planner_data.path.points, optimized_traj_points); - // 3. generate post processed trajectory - const auto post_processed_traj_points = - generatePostProcessedTrajectory(planner_data, extended_traj_points); + // 5. set zero velocity after stop point + setZeroVelocityAfterStopPoint(extended_traj_points); - // 4. publish debug data + // 6. publish debug data publishDebugDataInMain(*path_ptr); debug_data_ptr_->msg_stream << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n" @@ -241,7 +252,7 @@ void CollisionFreePathPlanner::onPath(const Path::SharedPtr path_ptr) debug_calculation_time_pub_->publish(calculation_time_msg); const auto output_traj_msg = - points_utils::createTrajectory(path_ptr->header, post_processed_traj_points); + trajectory_utils::createTrajectory(path_ptr->header, extended_traj_points); traj_pub_->publish(output_traj_msg); } @@ -280,30 +291,27 @@ PlannerData CollisionFreePathPlanner::createPlannerData(const Path & path) } std::vector CollisionFreePathPlanner::generateOptimizedTrajectory( - const PlannerData & planner_data) + const PlannerData & planner_data, const CVMaps & cv_maps) { stop_watch_.tic(__func__); const auto & path = planner_data.path; - // 1. create clearance maps - const CVMaps cv_maps = costmap_generator_ptr_->getMaps(planner_data, traj_param_); - - // 2. calculate trajectory with EB and MPT + // 1. calculate trajectory with EB and MPT // NOTE: This function may return previously optimized trajectory points. auto optimized_traj_points = optimizeTrajectory(planner_data, cv_maps); - // 3. update velocity + // 2. update velocity // Even if optimization is skipped, velocity in trajectory points must be updated since velocity // in input path may change - updateVelocity(optimized_traj_points, path.points); + applyPathVelocity(optimized_traj_points, path.points); - // 4. insert zero velocity when trajectory is over drivable area + // 3. insert zero velocity when trajectory is over drivable area if (enable_outside_drivable_area_stop_) { insertZeroVelocityOutsideDrivableArea(planner_data, optimized_traj_points, cv_maps); } - // 5. publish debug marker + // 4. publish debug marker publishDebugMarkerInOptimization(planner_data, optimized_traj_points); debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) @@ -332,7 +340,7 @@ std::vector CollisionFreePathPlanner::optimizeTrajectory( } if (enable_skip_optimization_) { - return points_utils::convertToTrajectoryPoints(p.path.points); + return trajectory_utils::convertToTrajectoryPoints(p.path.points); } // 2. Elastic Band: smooth trajectory if enable_smoothing is true @@ -343,7 +351,7 @@ if (enable_smoothing_) { return eb_path_optimizer_ptr_->getEBTrajectory(planner_data, prev_eb_traj_ptr_); } */ - return points_utils::convertToTrajectoryPoints(p.path.points); + return trajectory_utils::convertToTrajectoryPoints(p.path.points); }(); if (!eb_traj) { return getPrevOptimizedTrajectory(p.path.points); @@ -387,16 +395,16 @@ std::vector CollisionFreePathPlanner::getPrevOptimizedTrajector } // TODO(murooka) no need to generate post procesed trajectory? - return points_utils::convertToTrajectoryPoints(path_points); + return trajectory_utils::convertToTrajectoryPoints(path_points); } -void CollisionFreePathPlanner::updateVelocity( +void CollisionFreePathPlanner::applyPathVelocity( std::vector & traj_points, const std::vector & path_points) const { stop_watch_.tic(__func__); for (size_t i = 0; i < traj_points.size(); i++) { - const size_t nearest_seg_idx = points_utils::findSoftNearestSegmentIndex( + const size_t nearest_seg_idx = trajectory_utils::findSoftNearestSegmentIndex( path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); @@ -408,6 +416,8 @@ void CollisionFreePathPlanner::updateVelocity( traj_points.at(i).longitudinal_velocity_mps = std::max( path_points.at(nearest_seg_idx).longitudinal_velocity_mps, path_points.at(max_idx).longitudinal_velocity_mps); + + // TODO(murooka) insert stop point explicitly } debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) @@ -426,25 +436,18 @@ void CollisionFreePathPlanner::insertZeroVelocityOutsideDrivableArea( // 1. calculate ego_index nearest to mpt_traj_points const size_t ego_idx = - points_utils::findEgoIndex(mpt_traj_points, planner_data.ego_pose, ego_nearest_param_); + trajectory_utils::findEgoIndex(mpt_traj_points, planner_data.ego_pose, ego_nearest_param_); // 2. calculate an end point to check being outside the drivable area // NOTE: Some end trajectory points will be ignored to check if outside the drivable area // since these points might be outside drivable area if only end reference points have high // curvature. - const size_t end_idx = [&]() { - const size_t enough_long_points_num = - static_cast(traj_param_.num_sampling_points * 0.7); - constexpr size_t end_ignored_points_num = 5; - if (mpt_traj_points.size() > enough_long_points_num) { - return std::max(static_cast(0), mpt_traj_points.size() - end_ignored_points_num); - } - return mpt_traj_points.size(); - }(); + const int end_idx = std::max(10, static_cast(mpt_traj_points.size()) - 5); // 3. assign zero velocity to the first point being outside the drivable area - debug_data_ptr_->stop_pose_by_drivable_area = boost::none; - for (auto & traj_point : mpt_traj_points) { + for (size_t i = ego_idx; i < static_cast(end_idx); ++i) { + auto & traj_point = mpt_traj_points.at(i); + // check if the footprint is outside the drivable area const bool is_outside = cv_drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( traj_point.pose, cv_maps, vehicle_info_); @@ -470,233 +473,95 @@ void CollisionFreePathPlanner::publishDebugMarkerInOptimization( const auto & p = planner_data; - if (enable_pub_debug_marker_) { - // debug marker - stop_watch_.tic("getDebugMarker"); - const auto & debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, false); - debug_data_ptr_->msg_stream << " getDebugMarker:= " << stop_watch_.toc("getDebugMarker") + if (!enable_pub_debug_marker_) { + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - - stop_watch_.tic("publishDebugMarker"); - debug_markers_pub_->publish(debug_marker); - debug_data_ptr_->msg_stream << " publishDebugMarker:= " - << stop_watch_.toc("publishDebugMarker") << " [ms]\n"; - - // debug wall marker - stop_watch_.tic("getAndPublishDebugWallMarker"); - const auto virtual_wall_marker_array = [&]() { - if (debug_data_ptr_->stop_pose_by_drivable_area) { - const auto & stop_pose = debug_data_ptr_->stop_pose_by_drivable_area.get(); - return motion_utils::createStopVirtualWallMarker( - stop_pose, "drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); - } - return motion_utils::createDeletedStopVirtualWallMarker(now(), 0); - }(); - - debug_wall_markers_pub_->publish(virtual_wall_marker_array); - debug_data_ptr_->msg_stream << " getAndPublishDebugWallMarker:= " - << stop_watch_.toc("getAndPublishDebugWallMarker") << " [ms]\n"; + return; } + // debug marker + stop_watch_.tic("getDebugMarker"); + const auto & debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, false); + debug_data_ptr_->msg_stream << " getDebugMarker:= " << stop_watch_.toc("getDebugMarker") + << " [ms]\n"; + + stop_watch_.tic("publishDebugMarker"); + debug_markers_pub_->publish(debug_marker); + debug_data_ptr_->msg_stream << " publishDebugMarker:= " + << stop_watch_.toc("publishDebugMarker") << " [ms]\n"; + + // debug wall marker + stop_watch_.tic("getAndPublishDebugWallMarker"); + const auto virtual_wall_marker_array = [&]() { + if (debug_data_ptr_->stop_pose_by_drivable_area) { + const auto & stop_pose = debug_data_ptr_->stop_pose_by_drivable_area.get(); + return motion_utils::createStopVirtualWallMarker( + stop_pose, "drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); + } + return motion_utils::createDeletedStopVirtualWallMarker(now(), 0); + }(); + + debug_wall_markers_pub_->publish(virtual_wall_marker_array); + debug_data_ptr_->msg_stream << " getAndPublishDebugWallMarker:= " + << stop_watch_.toc("getAndPublishDebugWallMarker") << " [ms]\n"; + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } std::vector CollisionFreePathPlanner::extendTrajectory( - const std::vector & path_points, const std::vector & optimized_points) + const std::vector & path_points, + const std::vector & optimized_traj_points) { stop_watch_.tic(__func__); + // guard if (path_points.empty()) { - return optimized_points; + return optimized_traj_points; } - if (optimized_points.empty()) { - return points_utils::convertToTrajectoryPoints(path_points); + if (optimized_traj_points.empty()) { + return trajectory_utils::convertToTrajectoryPoints(path_points); } - /* - const double accum_arc_length = motion_utils::calcArcLength(optimized_points); - if (accum_arc_length > traj_param_.output_traj_length) { - RCLCPP_INFO_THROTTLE( - get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), - "[Avoidance] Not extend trajectory"); - return std::vector{}; - } - */ + const auto & joint_start_pose = optimized_traj_points.back().pose; // calculate end idx of optimized points on path points const auto opt_end_path_seg_idx = motion_utils::findNearestSegmentIndex( - path_points, optimized_points.back().pose, std::numeric_limits::max(), + path_points, joint_start_pose, traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); if (!opt_end_path_seg_idx) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), std::chrono::seconds(5).count(), - "[Avoidance] Not extend trajectory since could not find nearest idx from last opt point"); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("mpt_optimizer"), enable_debug_info_, + "Not extend trajectory since could not find nearest idx from last opt point"); return std::vector{}; } const size_t end_path_seg_idx = opt_end_path_seg_idx.get(); - auto extended_traj_points = optimized_points; - const size_t extended_start_point_idx = end_path_seg_idx + 1; - for (size_t i = extended_start_point_idx; i < path_points.size(); ++i) { - const auto extended_traj_point = points_utils::convertToTrajectoryPoint(path_points.at(i)); - extended_traj_points.push_back(extended_traj_point); - } - - // TODO(murooka) resample - - // TODO(murooka) update vleocity on joint - - // TODO(murooka) compensate goal pose - - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return extended_traj_points; -} + // crop forward trajectory + const auto forward_traj_points = [&]() -> std::vector { + constexpr double joint_traj_length_for_resampling = 10.0; + const auto forward_path_points = trajectory_utils::cropPointsAfterOffsetPoint( + path_points, joint_start_pose.position, end_path_seg_idx, joint_traj_length_for_resampling); -std::vector CollisionFreePathPlanner::generatePostProcessedTrajectory( - const PlannerData & planner_data, const std::vector & optimized_traj_points) -{ - stop_watch_.tic(__func__); - - const auto & p = planner_data; - - if (p.path.points.empty()) { - return std::vector{}; - } - if (optimized_traj_points.empty()) { - return points_utils::convertToTrajectoryPoints(p.path.points); - } - - auto traj_points = optimized_traj_points; - - // insert zero velocity after stop point - // NOTE: This implementation is required for stopping due to going outside the drivable area. - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); - if (opt_zero_vel_idx) { - for (size_t i = opt_zero_vel_idx.get(); i < traj_points.size(); ++i) { - traj_points.at(i).longitudinal_velocity_mps = 0.0; + if (forward_path_points.empty()) { // compensate goal pose + // TODO(murooka) optimization last point may be path last point + std::vector{trajectory_utils::convertToTrajectoryPoint(path_points.back())}; } - } - - /* - // concat trajectories - const auto traj_points = concatVectors(traj_points, extended_traj_points); - // NOTE: fine_traj_points has no velocity information - const auto fine_traj_points = generateFineTrajectoryPoints(path_points, traj_points); - - const auto fine_traj_points_with_vel = - alignVelocity(fine_traj_points, path_points, traj_points); - */ - - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return traj_points; -} - -/* -std::vector CollisionFreePathPlanner::alignVelocity( - const std::vector & fine_traj_points, const std::vector & path_points, - const std::vector & traj_points) const -{ - stop_watch_.tic(__func__); - - // insert zero velocity path index, and get optional zero_vel_path_idx - const auto path_zero_vel_info = - [&]() -> std::pair, boost::optional> { - const auto opt_path_zero_vel_idx = motion_utils::searchZeroVelocityIndex(path_points); - if (opt_path_zero_vel_idx) { - const auto & zero_vel_path_point = path_points.at(opt_path_zero_vel_idx.get()); - const auto opt_traj_seg_idx = motion_utils::findNearestSegmentIndex( - fine_traj_points, zero_vel_path_point.pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - if (opt_traj_seg_idx) { - const auto interpolated_pose = - lerpPose(fine_traj_points, zero_vel_path_point.pose.position, opt_traj_seg_idx.get()); - if (interpolated_pose) { - TrajectoryPoint zero_vel_traj_point; - zero_vel_traj_point.pose = interpolated_pose.get(); - zero_vel_traj_point.longitudinal_velocity_mps = - zero_vel_path_point.longitudinal_velocity_mps; - - if ( - tier4_autoware_utils::calcDistance2d( - fine_traj_points.at(opt_traj_seg_idx.get()).pose, zero_vel_traj_point.pose) < 1e-3) { - return {fine_traj_points, opt_traj_seg_idx.get()}; - } else if ( - tier4_autoware_utils::calcDistance2d( - fine_traj_points.at(opt_traj_seg_idx.get() + 1).pose, zero_vel_traj_point.pose) < - 1e-3) { - return {fine_traj_points, opt_traj_seg_idx.get() + 1}; - } - - auto fine_traj_points_with_zero_vel = fine_traj_points; - fine_traj_points_with_zero_vel.insert( - fine_traj_points_with_zero_vel.begin() + opt_traj_seg_idx.get() + 1, - zero_vel_traj_point); - return {fine_traj_points_with_zero_vel, opt_traj_seg_idx.get() + 1}; - } - } - } - - return {fine_traj_points, {}}; - }(); - const auto fine_traj_points_with_path_zero_vel = path_zero_vel_info.first; - const auto opt_zero_vel_path_idx = path_zero_vel_info.second; - - // search zero velocity index of fine_traj_points - const size_t zero_vel_fine_traj_idx = [&]() { - // zero velocity for being outside drivable area - const size_t zero_vel_traj_idx = searchExtendedZeroVelocityIndex( - fine_traj_points_with_path_zero_vel, traj_points, - traj_param_.delta_yaw_threshold_for_closest_point); - - // zero velocity in path points - if (opt_zero_vel_path_idx) { - return std::min(opt_zero_vel_path_idx.get(), zero_vel_traj_idx); - } - return zero_vel_traj_idx; + return trajectory_utils::convertToTrajectoryPoints(forward_path_points); }(); - // interpolate z and velocity - auto fine_traj_points_with_vel = fine_traj_points_with_path_zero_vel; - size_t prev_begin_idx = 0; - for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { - auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); - if (truncated_points.size() < 2) { - // NOTE: At least, two points must be contained in truncated_points - truncated_points = std::vector( - path_points.begin() + prev_begin_idx, - path_points.begin() + std::min(path_points.size(), prev_begin_idx + 2)); - } - - const auto & target_pose = fine_traj_points_with_vel[i].pose; - const size_t closest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( - truncated_points, target_pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - - // lerp z - fine_traj_points_with_vel[i].pose.position.z = - lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx); + const auto extended_traj_points = concatVectors(optimized_traj_points, forward_traj_points); - // lerp vx - const double target_vel = lerpTwistX(truncated_points, target_pose.position, closest_seg_idx); + const auto resampled_traj_points = trajectory_utils::resampleTrajectoryPoints( + extended_traj_points, traj_param_.output_delta_arc_length); - if (i >= zero_vel_fine_traj_idx) { - fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0; - } else { - fine_traj_points_with_vel[i].longitudinal_velocity_mps = target_vel; - } - - // NOTE: closest_seg_idx is for the clipped trajectory. This operation must be "+=". - prev_begin_idx += closest_seg_idx; - } + // TODO(murooka) update velocity on joint - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " -[ms]\n"; return fine_traj_points_with_vel; + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return resampled_traj_points; } -*/ void CollisionFreePathPlanner::publishDebugDataInMain(const Path & path) const { @@ -704,11 +569,11 @@ void CollisionFreePathPlanner::publishDebugDataInMain(const Path & path) const // publish trajectories const auto debug_extended_fixed_traj = - points_utils::createTrajectory(path.header, debug_data_ptr_->extended_fixed_traj); + trajectory_utils::createTrajectory(path.header, debug_data_ptr_->extended_fixed_traj); debug_extended_fixed_traj_pub_->publish(debug_extended_fixed_traj); const auto debug_extended_non_fixed_traj = - points_utils::createTrajectory(path.header, debug_data_ptr_->extended_non_fixed_traj); + trajectory_utils::createTrajectory(path.header, debug_data_ptr_->extended_non_fixed_traj); debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) diff --git a/planning/collision_free_path_planner/src/replan_checker.cpp b/planning/collision_free_path_planner/src/replan_checker.cpp index 4adedfa9cf241..33d3de50844ea 100644 --- a/planning/collision_free_path_planner/src/replan_checker.cpp +++ b/planning/collision_free_path_planner/src/replan_checker.cpp @@ -14,6 +14,7 @@ #include "collision_free_path_planner/replan_checker.hpp" +#include "collision_free_path_planner/utils/trajectory_utils.hpp" #include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -127,14 +128,15 @@ bool ReplanChecker::isPathShapeChanged( // truncate prev points from ego pose to fixed end points const auto prev_begin_idx = - points_utils::findEgoIndex(prev_path_points, p.ego_pose, ego_nearest_param_); + trajectory_utils::findEgoIndex(prev_path_points, p.ego_pose, ego_nearest_param_); const auto truncated_prev_points = - points_utils::clipForwardPoints(prev_path_points, prev_begin_idx, max_path_length); + trajectory_utils::clipForwardPoints(prev_path_points, prev_begin_idx, max_path_length); // truncate points from ego pose to fixed end points - const auto begin_idx = points_utils::findEgoIndex(p.path.points, p.ego_pose, ego_nearest_param_); + const auto begin_idx = + trajectory_utils::findEgoIndex(p.path.points, p.ego_pose, ego_nearest_param_); const auto truncated_points = - points_utils::clipForwardPoints(p.path.points, begin_idx, max_path_length); + trajectory_utils::clipForwardPoints(p.path.points, begin_idx, max_path_length); // guard for lateral offset if (truncated_prev_points.size() < 2 || truncated_points.size() < 2) { diff --git a/planning/collision_free_path_planner/src/utils/cv_utils.cpp b/planning/collision_free_path_planner/src/utils/cv_utils.cpp index 8fc9fed615071..36e847afc33b7 100644 --- a/planning/collision_free_path_planner/src/utils/cv_utils.cpp +++ b/planning/collision_free_path_planner/src/utils/cv_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 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. @@ -14,7 +14,7 @@ #include "collision_free_path_planner/utils/cv_utils.hpp" -#include "collision_free_path_planner/utils/utils.hpp" +#include "collision_free_path_planner/utils/geometry_utils.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/system/stop_watch.hpp" diff --git a/planning/collision_free_path_planner/src/utils/geometry_utils.cpp b/planning/collision_free_path_planner/src/utils/geometry_utils.cpp new file mode 100644 index 0000000000000..6d8700b5a9cd4 --- /dev/null +++ b/planning/collision_free_path_planner/src/utils/geometry_utils.cpp @@ -0,0 +1,125 @@ +// Copyright 2022 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 "collision_free_path_planner/utils/geometry_utils.hpp" + +#include "collision_free_path_planner/eb_path_optimizer.hpp" +#include "collision_free_path_planner/mpt_optimizer.hpp" +#include "motion_utils/motion_utils.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include +#include +#include + +namespace collision_free_path_planner +{ +namespace geometry_utils +{ +// TODO(murooka) check if this can be replaced with calcOffsetPose considering calculation time +geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin) +{ + // NOTE: implement transformation without defining yaw variable + // but directly sin/cos of yaw for fast calculation + const auto & q = origin.orientation; + const double cos_yaw = 1 - 2 * q.z * q.z; + const double sin_yaw = 2 * q.w * q.z; + + geometry_msgs::msg::Point absolute_p; + absolute_p.x = point.x * cos_yaw - point.y * sin_yaw + origin.position.x; + absolute_p.y = point.x * sin_yaw + point.y * cos_yaw + origin.position.y; + absolute_p.z = point.z; + + return absolute_p; +} + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) +{ + const double yaw = tier4_autoware_utils::calcAzimuthAngle(a_root, a); + return tier4_autoware_utils::createQuaternionFromYaw(yaw); +} + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + const double dx = (8.0 * (p3.x - p2.x) - (p4.x - p1.x)) / 12.0; + const double dy = (8.0 * (p3.y - p2.y) - (p4.y - p1.y)) / 12.0; + const double yaw = std::atan2(dy, dx); + + return tier4_autoware_utils::createQuaternionFromYaw(yaw); +} + +boost::optional transformMapToOptionalImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info) +{ + const geometry_msgs::msg::Point relative_p = + transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); + const double resolution = occupancy_grid_info.resolution; + const double map_y_height = occupancy_grid_info.height; + const double map_x_width = occupancy_grid_info.width; + const double map_x_in_image_resolution = relative_p.x / resolution; + const double map_y_in_image_resolution = relative_p.y / resolution; + const double image_x = map_y_height - map_y_in_image_resolution; + const double image_y = map_x_width - map_x_in_image_resolution; + if ( + image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && + image_y < static_cast(map_x_width)) { + geometry_msgs::msg::Point image_point; + image_point.x = image_x; + image_point.y = image_y; + return image_point; + } else { + return boost::none; + } +} + +bool transformMapToImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point) +{ + geometry_msgs::msg::Point relative_p = + transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); + const double map_y_height = occupancy_grid_info.height; + const double map_x_width = occupancy_grid_info.width; + const double scale = 1 / occupancy_grid_info.resolution; + const double map_x_in_image_resolution = relative_p.x * scale; + const double map_y_in_image_resolution = relative_p.y * scale; + const double image_x = map_y_height - map_y_in_image_resolution; + const double image_y = map_x_width - map_x_in_image_resolution; + if ( + image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && + image_y < static_cast(map_x_width)) { + image_point.x = image_x; + image_point.y = image_y; + return true; + } else { + return false; + } +} +} // namespace geometry_utils +} // namespace collision_free_path_planner diff --git a/planning/collision_free_path_planner/src/utils/trajectory_utils.cpp b/planning/collision_free_path_planner/src/utils/trajectory_utils.cpp new file mode 100644 index 0000000000000..ac5989d4f9820 --- /dev/null +++ b/planning/collision_free_path_planner/src/utils/trajectory_utils.cpp @@ -0,0 +1,164 @@ +// Copyright 2022 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 "collision_free_path_planner/utils/trajectory_utils.hpp" + +#include "collision_free_path_planner/eb_path_optimizer.hpp" +#include "collision_free_path_planner/mpt_optimizer.hpp" +#include "motion_utils/motion_utils.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const collision_free_path_planner::ReferencePoint & p) +{ + return p.p; +} + +template <> +geometry_msgs::msg::Pose getPose(const collision_free_path_planner::ReferencePoint & p) +{ + geometry_msgs::msg::Pose pose; + pose.position = p.p; + pose.orientation = createQuaternionFromYaw(p.yaw); + return pose; +} +} // namespace tier4_autoware_utils + +namespace collision_free_path_planner +{ +namespace trajectory_utils +{ +// functions to convert to another type of points +std::vector convertToPosesWithYawEstimation( + const std::vector points) +{ + std::vector poses; + if (points.empty()) { + return poses; + } else if (points.size() == 1) { + geometry_msgs::msg::Pose pose; + pose.position = points.at(0); + poses.push_back(pose); + return poses; + } + + for (size_t i = 0; i < points.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position = points.at(i); + + const size_t front_idx = (i == points.size() - 1) ? i - 1 : i; + const double points_yaw = + tier4_autoware_utils::calcAzimuthAngle(points.at(front_idx), points.at(front_idx + 1)); + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(points_yaw); + + poses.push_back(pose); + } + return poses; +} + +ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point) +{ + ReferencePoint ref_point; + + ref_point.p = traj_point.pose.position; + ref_point.yaw = tf2::getYaw(traj_point.pose.orientation); + ref_point.v = traj_point.longitudinal_velocity_mps; + + return ref_point; +} + +std::vector convertToReferencePoints( + const std::vector & traj_points) +{ + std::vector ref_points; + for (const auto & traj_point : traj_points) { + const auto ref_point = convertToReferencePoint(traj_point); + ref_points.push_back(ref_point); + } + + return ref_points; +} + +void compensateLastPose( + const PathPoint & last_path_point, std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold) +{ + if (traj_points.empty()) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + return; + } + + const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; + + const double dist = + tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double norm_diff_yaw = [&]() { + const double diff_yaw = + tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); + return tier4_autoware_utils::normalizeRadian(diff_yaw); + }(); + if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + } +} + +geometry_msgs::msg::Point getNearestPosition( + const std::vector & points, const int target_idx, const double offset) +{ + double sum_arc_length = 0.0; + for (size_t i = target_idx; i < points.size(); ++i) { + sum_arc_length += points.at(target_idx).delta_arc_length; + + if (offset < sum_arc_length) { + return points.at(target_idx).p; + } + } + + return points.back().p; +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & traj_points) +{ + auto traj = motion_utils::convertToTrajectory(traj_points); + traj.header = header; + + return traj; +} + +std::vector resampleTrajectoryPoints( + const std::vector traj_points, const double interval) +{ + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} +} // namespace trajectory_utils +} // namespace collision_free_path_planner diff --git a/planning/collision_free_path_planner/src/utils/utils.cpp b/planning/collision_free_path_planner/src/utils/utils.cpp deleted file mode 100644 index 374e88853eb49..0000000000000 --- a/planning/collision_free_path_planner/src/utils/utils.cpp +++ /dev/null @@ -1,569 +0,0 @@ -// 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 "collision_free_path_planner/utils/utils.hpp" - -#include "collision_free_path_planner/eb_path_optimizer.hpp" -#include "collision_free_path_planner/mpt_optimizer.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tf2/utils.h" - -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include -#include -#include - -namespace tier4_autoware_utils -{ -template <> -geometry_msgs::msg::Point getPoint(const collision_free_path_planner::ReferencePoint & p) -{ - return p.p; -} - -template <> -geometry_msgs::msg::Pose getPose(const collision_free_path_planner::ReferencePoint & p) -{ - geometry_msgs::msg::Pose pose; - pose.position = p.p; - pose.orientation = createQuaternionFromYaw(p.yaw); - return pose; -} -} // namespace tier4_autoware_utils - -namespace collision_free_path_planner -{ -namespace -{ -std::vector convertEulerAngleToMonotonic(const std::vector & angle) -{ - if (angle.empty()) { - return std::vector{}; - } - - std::vector monotonic_angle{angle.front()}; - for (size_t i = 1; i < angle.size(); ++i) { - const double diff_angle = angle.at(i) - monotonic_angle.back(); - monotonic_angle.push_back( - monotonic_angle.back() + tier4_autoware_utils::normalizeRadian(diff_angle)); - } - - return monotonic_angle; -} - -std::vector calcEuclidDist(const std::vector & x, const std::vector & y) -{ - if (x.size() != y.size()) { - std::cerr << "x y vector size should be the same." << std::endl; - } - - std::vector dist_v; - dist_v.push_back(0.0); - for (unsigned int i = 0; i < x.size() - 1; ++i) { - const double dx = x.at(i + 1) - x.at(i); - const double dy = y.at(i + 1) - y.at(i); - dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); - } - - return dist_v; -} - -std::array, 3> validateTrajectoryPoints( - const std::vector & points) -{ - constexpr double epsilon = 1e-6; - - std::vector x; - std::vector y; - std::vector yaw; - for (size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < epsilon && - std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < epsilon) { - continue; - } - } - x.push_back(points[i].pose.position.x); - y.push_back(points[i].pose.position.y); - yaw.push_back(tf2::getYaw(points[i].pose.orientation)); - } - return {x, y, yaw}; -} - -std::array, 2> validatePoints(const std::vector & points) -{ - std::vector x; - std::vector y; - for (size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < 1e-6 && - std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < 1e-6) { - continue; - } - } - x.push_back(points[i].pose.position.x); - y.push_back(points[i].pose.position.y); - } - return {x, y}; -} - -// only two points is supported -std::vector splineTwoPoints( - std::vector base_s, std::vector base_x, const double begin_diff, - const double end_diff, std::vector new_s) -{ - const double h = base_s.at(1) - base_s.at(0); - - const double c = begin_diff; - const double d = base_x.at(0); - const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); - const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); - - std::vector res; - for (const auto & s : new_s) { - const double ds = s - base_s.at(0); - res.push_back(d + (c + (b + a * ds) * ds) * ds); - } - - return res; -} -} // namespace - -namespace geometry_utils -{ -// TODO(murooka) check if this can be replaced with calcOffsetPose considering calculation time -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin) -{ - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point absolute_p; - absolute_p.x = point.x * cos_yaw - point.y * sin_yaw + origin.position.x; - absolute_p.y = point.x * sin_yaw + point.y * cos_yaw + origin.position.y; - absolute_p.z = point.z; - - return absolute_p; -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) -{ - const double yaw = tier4_autoware_utils::calcAzimuthAngle(a_root, a); - return tier4_autoware_utils::createQuaternionFromYaw(yaw); -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - const double dx = (8.0 * (p3.x - p2.x) - (p4.x - p1.x)) / 12.0; - const double dy = (8.0 * (p3.y - p2.y) - (p4.y - p1.y)) / 12.0; - const double yaw = std::atan2(dy, dx); - - return tier4_autoware_utils::createQuaternionFromYaw(yaw); -} - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - const geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - const double resolution = occupancy_grid_info.resolution; - const double map_y_height = occupancy_grid_info.height; - const double map_x_width = occupancy_grid_info.width; - const double map_x_in_image_resolution = relative_p.x / resolution; - const double map_y_in_image_resolution = relative_p.y / resolution; - const double image_x = map_y_height - map_y_in_image_resolution; - const double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - geometry_msgs::msg::Point image_point; - image_point.x = image_x; - image_point.y = image_y; - return image_point; - } else { - return boost::none; - } -} - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - const double map_y_height = occupancy_grid_info.height; - const double map_x_width = occupancy_grid_info.width; - const double scale = 1 / occupancy_grid_info.resolution; - const double map_x_in_image_resolution = relative_p.x * scale; - const double map_y_in_image_resolution = relative_p.y * scale; - const double image_x = map_y_height - map_y_in_image_resolution; - const double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - image_point.x = image_x; - image_point.y = image_y; - return true; - } else { - return false; - } -} -} // namespace geometry_utils - -/* -namespace interpolation_utils -{ -std::vector interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double offset = 0.0) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - const std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - - std::vector new_s; - for (double i = offset; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - if (new_s.empty()) { - return std::vector{}; - } - - // spline interpolation - const std::vector interpolated_x = interpolation::spline(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::spline(base_s, base_y, new_s); - for (size_t i = 0; i < interpolated_x.size(); ++i) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); ++i) { - geometry_msgs::msg::Point point; - point.x = interpolated_x[i]; - point.y = interpolated_y[i]; - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolateConnected2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - const double begin_yaw, const double end_yaw) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - // spline interpolation - const auto interpolated_x = - splineTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); - const auto interpolated_y = - splineTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - - const size_t front_idx = (i == interpolated_x.size() - 1) ? i - 1 : i; - const double dx = interpolated_x[front_idx + 1] - interpolated_x[front_idx]; - const double dy = interpolated_y[front_idx + 1] - interpolated_y[front_idx]; - const double yaw = std::atan2(dy, dx); - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, - const std::vector & base_yaw, const double resolution) -{ - if (base_x.empty() || base_y.empty() || base_yaw.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); - - // spline interpolation - const auto interpolated_x = interpolation::spline(base_s, base_x, new_s); - const auto interpolated_y = interpolation::spline(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::spline(base_s, monotonic_base_yaw, new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector interpolate2DTrajectoryPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution) -{ - if (base_x.empty() || base_y.empty()) { - return std::vector{}; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return std::vector{}; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - // spline interpolation - // x = interpolated[0], y = interpolated[1], yaw = interpolated[2] - std::array, 3> interpolated = - interpolation::slerp2dFromXY(base_s, base_x, base_y, new_s); - const auto & interpolated_x = interpolated[0]; - const auto & interpolated_y = interpolated[1]; - const auto & interpolated_yaw = interpolated[2]; - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; - } - } - - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); - } - - return interpolated_points; -} - -std::vector getInterpolatedTrajectoryPoints( - const std::vector & points, const double delta_arc_length) -{ - std::array, 3> validated_pose = validateTrajectoryPoints(points); - return interpolation_utils::interpolate2DTrajectoryPoints( - validated_pose.at(0), validated_pose.at(1), delta_arc_length); -} - -std::vector getConnectedInterpolatedPoints( - const std::vector & points, const double delta_arc_length, - const double begin_yaw, const double end_yaw) -{ - std::array, 2> validated_pose = validatePoints(points); - return interpolation_utils::interpolateConnected2DPoints( - validated_pose.at(0), validated_pose.at(1), delta_arc_length, begin_yaw, end_yaw); -} -} // namespace interpolation_utils -*/ - -namespace points_utils -{ -// functions to convert to another type of points -std::vector convertToPosesWithYawEstimation( - const std::vector points) -{ - std::vector poses; - if (points.empty()) { - return poses; - } else if (points.size() == 1) { - geometry_msgs::msg::Pose pose; - pose.position = points.at(0); - poses.push_back(pose); - return poses; - } - - for (size_t i = 0; i < points.size(); ++i) { - geometry_msgs::msg::Pose pose; - pose.position = points.at(i); - - const size_t front_idx = (i == points.size() - 1) ? i - 1 : i; - const double points_yaw = - tier4_autoware_utils::calcAzimuthAngle(points.at(front_idx), points.at(front_idx + 1)); - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(points_yaw); - - poses.push_back(pose); - } - return poses; -} - -ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point) -{ - ReferencePoint ref_point; - - ref_point.p = traj_point.pose.position; - ref_point.yaw = tf2::getYaw(traj_point.pose.orientation); - ref_point.v = traj_point.longitudinal_velocity_mps; - - return ref_point; -} - -std::vector convertToReferencePoints( - const std::vector & traj_points) -{ - std::vector ref_points; - for (const auto & traj_point : traj_points) { - const auto ref_point = convertToReferencePoint(traj_point); - ref_points.push_back(ref_point); - } - - return ref_points; -} - -/* -template -ReferencePoint convertToReferencePoint(const T & point) -{ - ReferencePoint ref_point; - - const auto & pose = tier4_autoware_utils::getPose(point); - ref_point.p = pose.position; - ref_point.yaw = tf2::getYaw(pose.orientation); - - return ref_point; -} - -template ReferencePoint convertToReferencePoint(const TrajectoryPoint & point); -template ReferencePoint convertToReferencePoint( - const geometry_msgs::msg::Pose & point); -template <> -ReferencePoint convertToReferencePoint(const geometry_msgs::msg::Point & point) -{ - ReferencePoint ref_point; - - ref_point.p = point; - - return ref_point; -} -*/ - -void compensateLastPose( - const PathPoint & last_path_point, std::vector & traj_points, - const double delta_dist_threshold, const double delta_yaw_threshold) -{ - if (traj_points.empty()) { - traj_points.push_back(convertToTrajectoryPoint(last_path_point)); - return; - } - - const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; - - const double dist = - tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); - const double norm_diff_yaw = [&]() { - const double diff_yaw = - tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return tier4_autoware_utils::normalizeRadian(diff_yaw); - }(); - if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { - traj_points.push_back(convertToTrajectoryPoint(last_path_point)); - } -} - -geometry_msgs::msg::Point getNearestPosition( - const std::vector & points, const int target_idx, const double offset) -{ - double sum_arc_length = 0.0; - for (size_t i = target_idx; i < points.size(); ++i) { - sum_arc_length += points.at(target_idx).delta_arc_length; - - if (offset < sum_arc_length) { - return points.at(target_idx).p; - } - } - - return points.back().p; -} - -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - -std::vector resampleTrajectoryPoints( - const std::vector traj_points, const double interval) -{ - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); -} -} // namespace points_utils -} // namespace collision_free_path_planner