diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt new file mode 100644 index 0000000000000..d24698f7d181e --- /dev/null +++ b/perception/map_based_prediction/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) +project(map_based_prediction) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(map_based_prediction_node SHARED + src/map_based_prediction_ros.cpp + src/map_based_prediction.cpp +) + +rclcpp_components_register_node(map_based_prediction_node + PLUGIN "MapBasedPredictionROS" + EXECUTABLE map_based_prediction +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md new file mode 100644 index 0000000000000..ee43ee89c81b1 --- /dev/null +++ b/perception/map_based_prediction/README.md @@ -0,0 +1,73 @@ +# map_based_prediction + +## Role + +`map_based_prediction` is a module to predict the future paths of other vehicles and pedestrians according to the shape of the map and the surrounding environment. + +## Inner-workings / Algorithms + +1. Get lanelet path + The first step is to get the lanelet of the current position of the car. After that, we obtain several trajectories based on the map. + +2. Lane Change Detection + After finding the current lanelet from the current position of the obstacle, our algorithm try to detect the lane change maneuver from the past positions of the obstacle. Our method uses the deviation between the obstacle's current position and its position one second ago and current position to determine if it is about to change lanes. The parameters used for the lane change decision are obtained by analyzing the data obtained from the experiment. We already confirmed that these parameters give the least number of false positives. + +3. Confidence calculation + We use the following metric to compute the distance to a certain lane. + + ```txt + d = x^T P x + ``` + + where `x=[lateral_dist, yaw_diff]` and `P` are covariance matrices. Therefore confidence values can be computed as + + ```txt + confidence = 1/d + ``` + + Finally, we normalize the confidence value to make it as probability value. Note that the standard deviation of the lateral distance and yaw difference is given by the user. + +4. Drawing predicted trajectories + From the current position and reference trajectories that we get in the step1, we create predicted trajectories by using Quintic polynomial. Note that, since this algorithm consider lateral and longitudinal motions separately, it sometimes generates dynamically-infeasible trajectories when the vehicle travels at a low speed. To deal with this problem, we only make straight line predictions when the vehicle speed is lower than a certain value (which is given as a parameter). + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------- | +| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. | +| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. | + +### Output + +| Name | Type | Description | +| ------------------------ | ------------------------------------------------------ | ------------------------------------- | +| `~/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | + +## Parameters + +| Parameter | Type | Description | +| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | +| `prediction_time_horizon` | double | predict time duration for predicted path [s] | +| `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | +| `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | +| `dist_threshold_for_searching_lanelet` | double | The threshold of the angle used when searching for the lane to which the object belongs [rad] | +| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | +| `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | +| `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | +| `history_time_length` | double | Time span of object information used for prediction [s] | +| `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | +| `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | +| `diff_dist_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | +| `diff_dist_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | + +## Assumptions / Known limits + +`map_based_prediction` can only predict future trajectories for cars, tracks and buses. + +## Reference + +1. M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenario in a frenet frame,” IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 2010. +2. A. Houenou, P. Bonnifait, V. Cherfaoui, and Wen Yao, “Vehicle trajectory prediction based on motion model and maneuver recognition,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, nov 2013, pp. 4363–4369. diff --git a/perception/map_based_prediction/include/cubic_spline.hpp b/perception/map_based_prediction/include/cubic_spline.hpp new file mode 100644 index 0000000000000..7e0826ceb1197 --- /dev/null +++ b/perception/map_based_prediction/include/cubic_spline.hpp @@ -0,0 +1,254 @@ +// Copyright 2018 Forrest +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef CUBIC_SPLINE_HPP_ +#define CUBIC_SPLINE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +static std::vector vec_diff(const std::vector & input) +{ + std::vector output; + for (unsigned int i = 1; i < input.size(); i++) { + output.push_back(input[i] - input[i - 1]); + } + return output; +} + +static std::vector cum_sum(const std::vector & input) +{ + std::vector output; + double temp = 0; + for (unsigned int i = 0; i < input.size(); i++) { + temp += input[i]; + output.push_back(temp); + } + return output; +} + +class Spline +{ +public: + std::vector x; + std::vector y; + int nx; + std::vector h; + std::vector a; + std::vector b; + std::vector c; + // Eigen::VectorXf c; + std::vector d; + + Spline() {} + // d_i * (x-x_i)^3 + c_i * (x-x_i)^2 + b_i * (x-x_i) + a_i + Spline(const std::vector & x_, const std::vector & y_) + : x(x_), y(y_), nx(x_.size()), h(vec_diff(x_)), a(y_) + { + Eigen::MatrixXd A = calc_A(); + Eigen::VectorXd B = calc_B(); + // std::cerr << "A det " << A.determinant() << std::endl; + // std::cerr << "A QR" << A.colPivHouseholderQr().solve(B) << std::endl; + Eigen::VectorXd c_eigen = A.colPivHouseholderQr().solve(B); + // std::cerr << "c eigen " << c_eigen << std::endl; + double * c_pointer = c_eigen.data(); + c.assign(c_pointer, c_pointer + c_eigen.rows()); + + for (int i = 0; i < nx - 1; i++) { + d.push_back((c[i + 1] - c[i]) / (3.0 * h[i])); + b.push_back((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2 * c[i]) / 3.0); + } + } + + double calc(double t) + { + if (t < x.front() || t > x.back()) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx; + } + + double calc(double t, double s) + { + if (t < 0 || t > s) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx; + } + + double calc_d(double t) + { + if (t < x.front() || t > x.back()) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx - 1); + double dx = t - x[seg_id]; + return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx; + } + + double calc_d(double t, double s) + { + if (t < 0 || t > s) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx - 1); + double dx = t - x[seg_id]; + return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx; + } + + double calc_dd(double t) + { + if (t < x.front() || t > x.back()) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return 2 * c[seg_id] + 6 * d[seg_id] * dx; + } + + double calc_dd(double t, double s) + { + if (t < 0.0 || t > s) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return 2 * c[seg_id] + 6 * d[seg_id] * dx; + } + +private: + Eigen::MatrixXd calc_A() + { + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(nx, nx); + A(0, 0) = 1; + for (int i = 0; i < nx - 1; i++) { + if (i != nx - 2) { + A(i + 1, i + 1) = 2 * (h[i] + h[i + 1]); + } + A(i + 1, i) = h[i]; + A(i, i + 1) = h[i]; + } + A(0, 1) = 0.0; + A(nx - 1, nx - 2) = 0.0; + A(nx - 1, nx - 1) = 1.0; + return A; + } + Eigen::VectorXd calc_B() + { + Eigen::VectorXd B = Eigen::VectorXd::Zero(nx); + for (int i = 0; i < nx - 2; i++) { + B(i + 1) = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] - 3.0 * (a[i + 1] - a[i]) / h[i]; + } + return B; + } + + int bisect(double t, int start, int end) + { + int mid = (start + end) / 2; + if (t == x[mid] || end - start <= 1) { + return mid; + } else if (t > x[mid]) { + return bisect(t, mid, end); + } else { + return bisect(t, start, mid); + } + } +}; + +class Spline2D +{ +public: + Spline sx; + Spline sy; + std::vector s; + double max_s_value_; + + Spline2D(const std::vector & x, const std::vector & y) + { + s = calc_s(x, y); + sx = Spline(s, x); + sy = Spline(s, y); + max_s_value_ = *std::max_element(s.begin(), s.end()); + } + + std::array calc_position(double s_t) + { + double x = sx.calc(s_t, max_s_value_); + double y = sy.calc(s_t, max_s_value_); + return {{x, y}}; + } + + double calc_curvature(double s_t) + { + double dx = sx.calc_d(s_t, max_s_value_); + double ddx = sx.calc_dd(s_t, max_s_value_); + double dy = sy.calc_d(s_t, max_s_value_); + double ddy = sy.calc_dd(s_t, max_s_value_); + return (ddy * dx - ddx * dy) / (dx * dx + dy * dy); + } + + double calc_yaw(double s_t) + { + double dx = sx.calc_d(s_t, max_s_value_); + double dy = sy.calc_d(s_t, max_s_value_); + return std::atan2(dy, dx); + } + +private: + std::vector calc_s(const std::vector & x, const std::vector & y) + { + std::vector ds; + std::vector out_s{0}; + std::vector dx = vec_diff(x); + std::vector dy = vec_diff(y); + + for (unsigned int i = 0; i < dx.size(); i++) { + ds.push_back(std::sqrt(dx[i] * dx[i] + dy[i] * dy[i])); + } + + std::vector cum_ds = cum_sum(ds); + out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end()); + return out_s; + } +}; + +#endif // CUBIC_SPLINE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction.hpp b/perception/map_based_prediction/include/map_based_prediction.hpp new file mode 100644 index 0000000000000..e6831c2be30f8 --- /dev/null +++ b/perception/map_based_prediction/include/map_based_prediction.hpp @@ -0,0 +1,79 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 MAP_BASED_PREDICTION_HPP_ +#define MAP_BASED_PREDICTION_HPP_ + +#include +#include +#include +#include + +#include + +struct DynamicObjectWithLanes +{ + autoware_auto_perception_msgs::msg::TrackedObject object; + std::vector> lanes; + std::vector confidence; +}; + +struct DynamicObjectWithLanesArray +{ + std_msgs::msg::Header header; + std::vector objects; +}; + +class Spline2D; + +class MapBasedPrediction +{ +private: + double interpolating_resolution_; + double time_horizon_; + double sampling_delta_time_; + + bool getPredictedPath( + const double height, const double current_d_position, const double current_d_velocity, + const double current_s_position, const double current_s_velocity, + const std_msgs::msg::Header & origin_header, Spline2D & spline2d, + autoware_auto_perception_msgs::msg::PredictedPath & path); + + void getLinearPredictedPath( + const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path); + + void normalizeLikelihood( + autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics); + + autoware_auto_perception_msgs::msg::PredictedObjectKinematics convertToPredictedKinematics( + const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object); + +public: + MapBasedPrediction( + double interpolating_resolution, double time_horizon, double sampling_delta_time); + + bool doPrediction( + const DynamicObjectWithLanesArray & in_objects, + std::vector & out_objects); + + bool doLinearPrediction( + const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, + std::vector & out_objects); + + autoware_auto_perception_msgs::msg::PredictedObject convertToPredictedObject( + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object); +}; + +#endif // MAP_BASED_PREDICTION_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction_ros.hpp b/perception/map_based_prediction/include/map_based_prediction_ros.hpp new file mode 100644 index 0000000000000..5f477f0133f79 --- /dev/null +++ b/perception/map_based_prediction/include/map_based_prediction_ros.hpp @@ -0,0 +1,158 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 MAP_BASED_PREDICTION_ROS_HPP_ +#define MAP_BASED_PREDICTION_ROS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace tf2_ros +{ +class Buffer; +class TransformListener; +} // namespace tf2_ros + +namespace lanelet +{ +class Lanelet; +class LaneletMap; +using LaneletMapPtr = std::shared_ptr; +namespace routing +{ +class RoutingGraph; +} // namespace routing +namespace traffic_rules +{ +class TrafficRules; +} // namespace traffic_rules +} // namespace lanelet + +struct ObjectData +{ + lanelet::ConstLanelets current_lanelets; + lanelet::ConstLanelets future_possible_lanelets; + geometry_msgs::msg::PoseStamped pose; +}; + +enum class Maneuver { + LANE_FOLLOW, + LEFT_LANE_CHANGE, + RIGHT_LANE_CHANGE, +}; + +class MapBasedPrediction; + +class MapBasedPredictionROS : public rclcpp::Node +{ +private: + double prediction_time_horizon_; + double prediction_sampling_delta_time_; + double min_velocity_for_map_based_prediction_; + double interpolating_resolution_; + double debug_accumulated_time_; + double dist_threshold_for_searching_lanelet_; + double delta_yaw_threshold_for_searching_lanelet_; + double sigma_lateral_offset_; + double sigma_yaw_angle_; + double history_time_length_; + double dist_ratio_threshold_to_left_bound_; + double dist_ratio_threshold_to_right_bound_; + double diff_dist_threshold_to_left_bound_; + double diff_dist_threshold_to_right_bound_; + + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Publisher::SharedPtr pub_objects_; + rclcpp::Publisher::SharedPtr pub_markers_; + + std::unordered_map> object_buffer_; + + std::shared_ptr tf_buffer_ptr_; + std::shared_ptr tf_listener_ptr_; + + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr routing_graph_ptr_; + std::shared_ptr traffic_rules_ptr_; + std::shared_ptr map_based_prediction_; + + bool getSelfPose(geometry_msgs::msg::Pose & self_pose, const std_msgs::msg::Header & header); + bool getSelfPoseInMap(geometry_msgs::msg::Pose & self_pose); + + double getObjectYaw(const autoware_auto_perception_msgs::msg::TrackedObject & object); + double calculateLikelihood( + const std::vector & path, + const autoware_auto_perception_msgs::msg::TrackedObject & object); + + void addValidPath( + const lanelet::routing::LaneletPaths & candidate_paths, + lanelet::routing::LaneletPaths & valid_paths); + + void objectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects); + void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + + bool getClosestLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::ConstLanelets & closest_lanelets); + + bool checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::BasicPoint2d & search_point); + + void removeInvalidObject(const double current_time); + bool updateObjectBuffer( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + lanelet::ConstLanelets & current_lanelets); + void updatePossibleLanelets( + const std::string object_id, const lanelet::routing::LaneletPaths & paths); + + double calcRightLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); + double calcLeftLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); + Maneuver detectLaneChange( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelet & current_lanelet, const double current_time); + +public: + explicit MapBasedPredictionROS(const rclcpp::NodeOptions & node_options); +}; + +#endif // MAP_BASED_PREDICTION_ROS_HPP_ diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml new file mode 100644 index 0000000000000..573fac87e379e --- /dev/null +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml new file mode 100644 index 0000000000000..b5e7f9ad941cf --- /dev/null +++ b/perception/map_based_prediction/package.xml @@ -0,0 +1,29 @@ + + + map_based_prediction + 0.1.0 + This package implements a map_based_prediction + + Kosuke Murakami + + Apache License 2.0 + + ament_cmake + + autoware_auto_perception_msgs + autoware_utils + lanelet2_extension + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + unique_identifier_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/map_based_prediction/src/map_based_prediction.cpp b/perception/map_based_prediction/src/map_based_prediction.cpp new file mode 100644 index 0000000000000..34e7b748faa3f --- /dev/null +++ b/perception/map_based_prediction/src/map_based_prediction.cpp @@ -0,0 +1,304 @@ +// 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 +#include +#include + +#include + +#include +#include +#include + +MapBasedPrediction::MapBasedPrediction( + double interpolating_resolution, double time_horizon, double sampling_delta_time) +: interpolating_resolution_(interpolating_resolution), + time_horizon_(time_horizon), + sampling_delta_time_(sampling_delta_time) +{ +} + +bool MapBasedPrediction::doPrediction( + const DynamicObjectWithLanesArray & in_objects, + std::vector & out_objects) +{ + for (auto & object_with_lanes : in_objects.objects) { + autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + tmp_object = convertToPredictedObject(object_with_lanes.object); + for (size_t path_id = 0; path_id < object_with_lanes.lanes.size(); ++path_id) { + std::vector tmp_x; + std::vector tmp_y; + std::vector path = object_with_lanes.lanes.at(path_id); + for (size_t i = 0; i < path.size(); i++) { + if (i > 0) { + double dist = autoware_utils::calcDistance2d(path.at(i), path.at(i - 1)); + if (dist < interpolating_resolution_) { + continue; + } + } + tmp_x.push_back(path.at(i).position.x); + tmp_y.push_back(path.at(i).position.y); + } + + // Generate Splined Trajectory Arc-Length Position and Yaw angle + Spline2D spline2d(tmp_x, tmp_y); + std::vector interpolated_points; + std::vector interpolated_yaws; + for (double s = 0.0; s < spline2d.s.back(); s += interpolating_resolution_) { + std::array point1 = spline2d.calc_position(s); + geometry_msgs::msg::Point g_point; + g_point.x = point1[0]; + g_point.y = point1[1]; + g_point.z = object_with_lanes.object.kinematics.pose_with_covariance.pose.position.z; + interpolated_points.push_back(g_point); + interpolated_yaws.push_back(spline2d.calc_yaw(s)); + } + + // calculate initial position in Frenet coordinate + // Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame + // Path Planning for Highly Automated Driving on Embedded GPUs + geometry_msgs::msg::Point object_point = + object_with_lanes.object.kinematics.pose_with_covariance.pose.position; + + const size_t nearest_segment_idx = + autoware_utils::findNearestSegmentIndex(interpolated_points, object_point); + const double l = autoware_utils::calcLongitudinalOffsetToSegment( + interpolated_points, nearest_segment_idx, object_point); + const double current_s_position = + autoware_utils::calcSignedArcLength(interpolated_points, 0, nearest_segment_idx) + l; + if (current_s_position > spline2d.s.back()) { + continue; + } + std::array s_point = spline2d.calc_position(current_s_position); + geometry_msgs::msg::Point nearest_point = + autoware_utils::createPoint(s_point[0], s_point[1], object_point.z); + double current_d_position = autoware_utils::calcDistance2d(nearest_point, object_point); + const double lane_yaw = spline2d.calc_yaw(current_s_position); + const std::vector origin_v = {std::cos(lane_yaw), std::sin(lane_yaw)}; + const std::vector object_v = { + object_point.x - nearest_point.x, object_point.y - nearest_point.y}; + const double cross2d = object_v.at(0) * origin_v.at(1) - object_v.at(1) * origin_v.at(0); + if (cross2d < 0) { + current_d_position *= -1; + } + + // Does not consider orientation of twist since predicting lane-direction + const double current_d_velocity = + object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.y; + const double current_s_velocity = + std::fabs(object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.x); + + // Predict Path + autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + getPredictedPath( + object_point.z, current_d_position, current_d_velocity, current_s_position, + current_s_velocity, in_objects.header, spline2d, predicted_path); + // substitute confidence value + predicted_path.confidence = object_with_lanes.confidence.at(path_id); + + // Check if the path is already generated + const double CLOSE_PATH_THRESHOLD = 0.1; + bool duplicate_flag = false; + for (const auto & prev_path : tmp_object.kinematics.predicted_paths) { + const auto prev_path_end = prev_path.path.back().position; + const auto current_path_end = predicted_path.path.back().position; + const double dist = autoware_utils::calcDistance2d(prev_path_end, current_path_end); + if (dist < CLOSE_PATH_THRESHOLD) { + duplicate_flag = true; + break; + } + } + + if (duplicate_flag) { + continue; + } + + tmp_object.kinematics.predicted_paths.push_back(predicted_path); + } + + normalizeLikelihood(tmp_object.kinematics); + out_objects.push_back(tmp_object); + } + return true; +} + +bool MapBasedPrediction::doLinearPrediction( + const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, + std::vector & out_objects) +{ + for (const auto & object : in_objects.objects) { + autoware_auto_perception_msgs::msg::PredictedPath path; + getLinearPredictedPath( + object.kinematics.initial_pose_with_covariance.pose, + object.kinematics.initial_twist_with_covariance.twist, path); + autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + tmp_object = object; + tmp_object.kinematics.predicted_paths.push_back(path); + out_objects.push_back(tmp_object); + } + + return true; +} + +autoware_auto_perception_msgs::msg::PredictedObject MapBasedPrediction::convertToPredictedObject( + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +{ + autoware_auto_perception_msgs::msg::PredictedObject output; + output.object_id = tracked_object.object_id; + output.existence_probability = tracked_object.existence_probability; + output.classification = tracked_object.classification; + output.kinematics = convertToPredictedKinematics(tracked_object.kinematics); + output.shape = tracked_object.shape; + return output; +} + +autoware_auto_perception_msgs::msg::PredictedObjectKinematics +MapBasedPrediction::convertToPredictedKinematics( + const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object) +{ + autoware_auto_perception_msgs::msg::PredictedObjectKinematics output; + output.initial_pose_with_covariance = tracked_object.pose_with_covariance; + output.initial_twist_with_covariance = tracked_object.twist_with_covariance; + output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; + return output; +} + +void MapBasedPrediction::normalizeLikelihood( + autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics) +{ + // might not be the smartest way + double sum_confidence = 0.0; + for (const auto & path : predicted_object_kinematics.predicted_paths) { + sum_confidence += path.confidence; + } + + for (auto & path : predicted_object_kinematics.predicted_paths) { + path.confidence = path.confidence / sum_confidence; + } +} + +bool MapBasedPrediction::getPredictedPath( + const double height, const double current_d_position, const double current_d_velocity, + const double current_s_position, const double current_s_velocity, + const std_msgs::msg::Header & origin_header, Spline2D & spline2d, + autoware_auto_perception_msgs::msg::PredictedPath & path) +{ + // Quintic polynomial for d + // A = np.array([[T**3, T**4, T**5], + // [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], + // [6 * T, 12 * T ** 2, 20 * T ** 3]]) + // A_inv = np.matrix([[10/(T**3), -4/(T**2), 1/(2*T)], + // [-15/(T**4), 7/(T**3), -1/(T**2)], + // [6/(T**5), -3/(T**4), 1/(2*T**3)]]) + // b = np.matrix([[xe - self.a0 - self.a1 * T - self.a2 * T**2], + // [vxe - self.a1 - 2 * self.a2 * T], + // [axe - 2 * self.a2]]) + double target_d_position = 0; + + double t = time_horizon_; + Eigen::Matrix3d a_3_inv; + a_3_inv << 10 / std::pow(t, 3), -4 / std::pow(t, 2), 1 / (2 * t), -15 / std::pow(t, 4), + 7 / std::pow(t, 3), -1 / std::pow(t, 2), 6 / std::pow(t, 5), -3 / std::pow(t, 4), + 1 / (2 * std::pow(t, 3)); + + double target_d_velocity = current_d_velocity; + double target_d_acceleration = 0; + Eigen::Vector3d b_3; + b_3 << target_d_position - current_d_position - current_d_velocity * t, + target_d_velocity - current_d_velocity, target_d_acceleration; + + Eigen::Vector3d x_3; + x_3 = a_3_inv * b_3; + + // Quadric polynomial + // A_inv = np.matrix([[1/(T**2), -1/(3*T)], + // [-1/(2*T**3), 1/(4*T**2)]]) + // b = np.matrix([[vxe - self.a1 - 2 * self.a2 * T], + // [axe - 2 * self.a2]]) + Eigen::Matrix2d a_2_inv; + a_2_inv << 1 / std::pow(t, 2), -1 / (3 * t), -1 / (2 * std::pow(t, 3)), 1 / (4 * std::pow(t, 2)); + double target_s_velocity = current_s_velocity; + Eigen::Vector2d b_2; + b_2 << target_s_velocity - current_s_velocity, 0; + Eigen::Vector2d x_2; + x_2 = a_2_inv * b_2; + + // sampling points from calculated path + double dt = sampling_delta_time_; + std::vector d_vec; + for (double i = 0; i < t; i += dt) { + const double calculated_d = current_d_position + current_d_velocity * i + 0 * 2 * i * i + + x_3(0) * i * i * i + x_3(1) * i * i * i * i + + x_3(2) * i * i * i * i * i; + const double calculated_s = current_s_position + current_s_velocity * i + 2 * 0 * i * i + + x_2(0) * i * i * i + x_2(1) * i * i * i * i; + + geometry_msgs::msg::Pose tmp_point; + if (calculated_s > spline2d.s.back()) { + break; + } + std::array p = spline2d.calc_position(calculated_s); + double yaw = spline2d.calc_yaw(calculated_s); + tmp_point.position.x = p[0] + std::cos(yaw - M_PI / 2.0) * calculated_d; + tmp_point.position.y = p[1] + std::sin(yaw - M_PI / 2.0) * calculated_d; + tmp_point.position.z = height; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, yaw); + tmp_point.orientation = tf2::toMsg(quat); + path.path.push_back(tmp_point); + if (path.path.size() >= path.path.max_size()) { + break; + } + } + + path.time_step = rclcpp::Duration::from_seconds(dt); + return true; +} + +void MapBasedPrediction::getLinearPredictedPath( + const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) +{ + const double & sampling_delta_time = sampling_delta_time_; + const double & time_horizon = time_horizon_; + const double ep = 0.001; + + for (double dt = 0.0; dt < time_horizon + ep; dt += sampling_delta_time) { + geometry_msgs::msg::Pose tmp_pose; + geometry_msgs::msg::Pose object_frame_pose; + geometry_msgs::msg::Pose world_frame_pose; + object_frame_pose.position.x = object_twist.linear.x * dt; + object_frame_pose.position.y = object_twist.linear.y * dt; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, 0.0); + object_frame_pose.orientation = tf2::toMsg(quat); + tf2::Transform tf_object2future; + tf2::Transform tf_world2object; + tf2::Transform tf_world2future; + + tf2::fromMsg(object_pose, tf_world2object); + tf2::fromMsg(object_frame_pose, tf_object2future); + tf_world2future = tf_world2object * tf_object2future; + tf2::toMsg(tf_world2future, world_frame_pose); + tmp_pose = world_frame_pose; + predicted_path.path.push_back(tmp_pose); + if (predicted_path.path.size() >= predicted_path.path.max_size()) { + break; + } + } + + predicted_path.confidence = 1.0; + predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_delta_time); +} diff --git a/perception/map_based_prediction/src/map_based_prediction_ros.cpp b/perception/map_based_prediction/src/map_based_prediction_ros.cpp new file mode 100644 index 0000000000000..d77603e70f2a0 --- /dev/null +++ b/perception/map_based_prediction/src/map_based_prediction_ros.cpp @@ -0,0 +1,760 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "map_based_prediction_ros.hpp" + +#include "map_based_prediction.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::string toHexString(const unique_identifier_msgs::msg::UUID & id) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; + } + return ss.str(); +} + +MapBasedPredictionROS::MapBasedPredictionROS(const rclcpp::NodeOptions & node_options) +: Node("map_based_prediction", node_options), + interpolating_resolution_(0.5), + debug_accumulated_time_(0.0) +{ + [[maybe_unused]] auto ret = + rcutils_logging_set_logger_level(this->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); + tf_buffer_ptr_ = std::make_shared(clock); + tf_listener_ptr_ = std::make_shared(*tf_buffer_ptr_); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon", 10.0); + prediction_sampling_delta_time_ = declare_parameter("prediction_sampling_delta_time", 0.5); + min_velocity_for_map_based_prediction_ = + declare_parameter("min_velocity_for_map_based_prediction", 1.0); + dist_threshold_for_searching_lanelet_ = + declare_parameter("dist_threshold_for_searching_lanelet", 3.0); + delta_yaw_threshold_for_searching_lanelet_ = + declare_parameter("delta_yaw_threshold_for_searching_lanelet", 0.785); + sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset", 0.5); + sigma_yaw_angle_ = declare_parameter("sigma_yaw_angle", 5.0); + history_time_length_ = declare_parameter("history_time_length", 1.0); + dist_ratio_threshold_to_left_bound_ = + declare_parameter("dist_ratio_threshold_to_left_bound", -0.5); + dist_ratio_threshold_to_right_bound_ = + declare_parameter("dist_ratio_threshold_to_right_bound", 0.5); + diff_dist_threshold_to_left_bound_ = declare_parameter("diff_dist_threshold_to_left_bound", 0.29); + diff_dist_threshold_to_right_bound_ = + declare_parameter("diff_dist_threshold_to_right_bound", -0.29); + + map_based_prediction_ = std::make_shared( + interpolating_resolution_, prediction_time_horizon_, prediction_sampling_delta_time_); + + sub_objects_ = this->create_subscription( + "/perception/object_recognition/tracking/objects", 1, + std::bind(&MapBasedPredictionROS::objectsCallback, this, std::placeholders::_1)); + sub_map_ = this->create_subscription( + "/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&MapBasedPredictionROS::mapCallback, this, std::placeholders::_1)); + + pub_objects_ = this->create_publisher( + "objects", rclcpp::QoS{1}); + pub_markers_ = this->create_publisher( + "objects_path_markers", rclcpp::QoS{1}); +} + +double MapBasedPredictionROS::getObjectYaw( + const autoware_auto_perception_msgs::msg::TrackedObject & object) +{ + if (object.kinematics.orientation_availability) { + return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + } + + geometry_msgs::msg::Pose object_frame_pose; + geometry_msgs::msg::Pose map_frame_pose; + object_frame_pose.position.x = object.kinematics.twist_with_covariance.twist.linear.x * 0.1; + object_frame_pose.position.y = object.kinematics.twist_with_covariance.twist.linear.y * 0.1; + tf2::Transform tf_object2future; + tf2::Transform tf_map2object; + tf2::Transform tf_map2future; + + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_map2object); + tf2::fromMsg(object_frame_pose, tf_object2future); + tf_map2future = tf_map2object * tf_object2future; + tf2::toMsg(tf_map2future, map_frame_pose); + double dx = map_frame_pose.position.x - object.kinematics.pose_with_covariance.pose.position.x; + double dy = map_frame_pose.position.y - object.kinematics.pose_with_covariance.pose.position.y; + return std::atan2(dy, dx); +} + +double MapBasedPredictionROS::calculateLikelihood( + const std::vector & path, + const autoware_auto_perception_msgs::msg::TrackedObject & object) +{ + // We compute the confidence value based on the object current position and angle + // Calculate path length + const double path_len = autoware_utils::calcArcLength(path); + const size_t nearest_segment_idx = autoware_utils::findNearestSegmentIndex( + path, object.kinematics.pose_with_covariance.pose.position); + const double l = autoware_utils::calcLongitudinalOffsetToSegment( + path, nearest_segment_idx, object.kinematics.pose_with_covariance.pose.position); + const double current_s_position = + autoware_utils::calcSignedArcLength(path, 0, nearest_segment_idx) + l; + // If the obstacle is ahead of this path, we assume the confidence for this path is 0 + if (current_s_position > path_len) { + return 0.0; + } + + // Euclid Lateral Distance + const double abs_d = std::fabs( + autoware_utils::calcLateralOffset(path, object.kinematics.pose_with_covariance.pose.position)); + + // Yaw Difference between obstacle and lane angle + const double lane_yaw = tf2::getYaw(path.at(nearest_segment_idx).orientation); + const double object_yaw = getObjectYaw(object); + const double delta_yaw = object_yaw - lane_yaw; + const double abs_norm_delta_yaw = std::fabs(autoware_utils::normalizeRadian(delta_yaw)); + + // Compute Chi-squared distributed (Equation (8) in the paper) + const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position + const double sigma_yaw = M_PI * sigma_yaw_angle_ / 180.0; // Standard Deviation for yaw angle + Eigen::Vector2d delta; + delta << abs_d, abs_norm_delta_yaw; + Eigen::Matrix2d P_inv; + P_inv << 1.0 / (sigma_d * sigma_d), 0.0, 0.0, 1.0 / (sigma_yaw * sigma_yaw); + const double MINIMUM_DISTANCE = 1e-6; + const double dist = std::max(delta.dot(P_inv * delta), MINIMUM_DISTANCE); + return 1.0 / dist; +} + +bool MapBasedPredictionROS::checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::BasicPoint2d & search_point) +{ + // Step1. If we only have one point in the centerline, we will ignore the lanelet + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + // Step2. Check if the obstacle is inside of this lanelet + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + return false; + } + + // If the object is in the object buffer, we check if the target lanelet is + // inside the current lanelets id or following lanelets + const std::string object_id = toHexString(object.object_id); + if (object_buffer_.count(object_id) != 0) { + const std::vector & possible_lanelet = + object_buffer_.at(object_id).back().future_possible_lanelets; + + bool not_in_possible_lanelet = + std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) == + possible_lanelet.end(); + if (not_in_possible_lanelet) { + return false; + } + } + + // Step3. Calculate the angle difference between the lane angle and obstacle angle + double object_yaw = getObjectYaw(object); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = std::atan2(std::sin(delta_yaw), std::cos(delta_yaw)); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + + // Step4. Check if the closest lanelet is valid, and add all + // of the lanelets that are below max_dist and max_delta_yaw + if ( + lanelet.first < dist_threshold_for_searching_lanelet_ && + abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_) { + return true; + } + + return false; +} + +bool MapBasedPredictionROS::getClosestLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr_, lanelet::ConstLanelets & closest_lanelets) +{ + std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); + + // obstacle point + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + // nearest lanelet + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10); + + std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); + std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); + debug_accumulated_time_ += time.count() / (1000.0 * 1000.0); + + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return false; + } + + const double MIN_DIST = 1e-6; + bool found_target_closest_lanelet = false; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets + if (checkCloseLaneletCondition(lanelet, object, search_point)) { + // If the lanelet meets the condition, + // then check if similar lanelet is inside the closest lanelet + bool is_duplicate = false; + for (const auto & closest_lanelet : closest_lanelets) { + const auto lanelet_end_p = lanelet.second.centerline2d().back(); + const auto closest_lanelet_end_p = closest_lanelet.centerline2d().back(); + const double dist = std::hypot( + lanelet_end_p.x() - closest_lanelet_end_p.x(), + lanelet_end_p.y() - closest_lanelet_end_p.y()); + if (dist < MIN_DIST) { + is_duplicate = true; + break; + } + } + if (is_duplicate) { + continue; + } + + found_target_closest_lanelet = true; + closest_lanelets.push_back(lanelet.second); + } + } + + // If the closest lanelet is valid, return true + return found_target_closest_lanelet; +} + +void MapBasedPredictionROS::removeInvalidObject(const double current_time) +{ + std::vector invalid_object_id; + for (auto iter = object_buffer_.begin(); iter != object_buffer_.end(); ++iter) { + const std::string object_id = iter->first; + std::deque & object_data = iter->second; + + // If object data is empty, we are going to delete the buffer for the obstacle + if (object_data.empty()) { + invalid_object_id.push_back(object_id); + continue; + } + const double latest_object_time = rclcpp::Time(object_data.back().pose.header.stamp).seconds(); + + // Delete Old Objects + if (current_time - latest_object_time > 2.0) { + invalid_object_id.push_back(object_id); + continue; + } + + // Delete old information + while (!object_data.empty()) { + const double post_object_time = rclcpp::Time(object_data.front().pose.header.stamp).seconds(); + if (current_time - post_object_time > 2.0) { + // Delete Old Position + object_data.pop_front(); + } else { + break; + } + } + + if (object_data.empty()) { + invalid_object_id.push_back(object_id); + continue; + } + } + + for (const auto & key : invalid_object_id) { + object_buffer_.erase(key); + } +} + +bool MapBasedPredictionROS::updateObjectBuffer( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + lanelet::ConstLanelets & current_lanelets) +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + // Ignore non-vehicle object + if ( + object.classification.front().label != Label::CAR && + object.classification.front().label != Label::BUS && + object.classification.front().label != Label::TRUCK) { + return false; + } + + // Get the current Lanelet + std::string object_id = toHexString(object.object_id); + + // Check whether the object is on the road + if (!getClosestLanelets(object, lanelet_map_ptr_, current_lanelets)) { + // This Object is not on the road + return false; + } + + // Get current Pose + geometry_msgs::msg::PoseStamped current_object_pose; + current_object_pose.header = header; + current_object_pose.pose = object.kinematics.pose_with_covariance.pose; + + // Update Object Buffer + if (object_buffer_.count(object_id) == 0) { + // New Object + ObjectData single_object_data; + single_object_data.current_lanelets = current_lanelets; + single_object_data.future_possible_lanelets = current_lanelets; + single_object_data.pose = current_object_pose; + const double object_yaw = getObjectYaw(object); + single_object_data.pose.pose.orientation = autoware_utils::createQuaternionFromYaw(object_yaw); + + std::deque object_data; + object_data.push_back(single_object_data); + + // Create new key, value pair in the buffer + object_buffer_.emplace(object_id, object_data); + } else { + // Object that is already in the object buffer + std::deque & object_data = object_buffer_.at(object_id); + + ObjectData single_object_data; + single_object_data.current_lanelets = current_lanelets; + single_object_data.future_possible_lanelets = current_lanelets; + single_object_data.pose = current_object_pose; + const double object_yaw = getObjectYaw(object); + single_object_data.pose.pose.orientation = autoware_utils::createQuaternionFromYaw(object_yaw); + + // push new object data + object_data.push_back(single_object_data); + } + + // If the obstacle is too slow, we do linear prediction + if ( + std::fabs(object.kinematics.twist_with_covariance.twist.linear.x) < + min_velocity_for_map_based_prediction_) { + return false; + } + + return true; +} + +void MapBasedPredictionROS::updatePossibleLanelets( + const std::string object_id, const lanelet::routing::LaneletPaths & paths) +{ + if (object_buffer_.count(object_id) != 0) { + std::vector & possible_lanelets = + object_buffer_.at(object_id).back().future_possible_lanelets; + for (const auto & path : paths) { + for (const auto & lanelet : path) { + bool not_in_buffer = + std::find(possible_lanelets.begin(), possible_lanelets.end(), lanelet) == + possible_lanelets.end(); + if (not_in_buffer) { + possible_lanelets.push_back(lanelet); + } + } + } + } +} + +void MapBasedPredictionROS::addValidPath( + const lanelet::routing::LaneletPaths & candidate_paths, + lanelet::routing::LaneletPaths & valid_paths) +{ + // Check if candidate paths are already in the valid paths + for (const auto & candidate_path : candidate_paths) { + bool already_searched = false; + for (const auto & valid_path : valid_paths) { + for (const auto & llt : valid_path) { + if (candidate_path.back().id() == llt.id()) { + already_searched = true; + } + } + } + if (!already_searched) { + valid_paths.push_back(candidate_path); + } + } +} + +double MapBasedPredictionROS::calcRightLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose) +{ + std::vector bound_path(bound_line.size()); + for (size_t i = 0; i < bound_path.size(); ++i) { + const double x = bound_line[i].x(); + const double y = bound_line[i].y(); + bound_path[i] = autoware_utils::createPoint(x, y, 0.0); + } + + return std::fabs(autoware_utils::calcLateralOffset(bound_path, search_pose.position)); +} + +double MapBasedPredictionROS::calcLeftLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose) +{ + return -calcRightLateralOffset(bound_line, search_pose); +} + +Maneuver MapBasedPredictionROS::detectLaneChange( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelet & current_lanelet, const double current_time) +{ + // Step1. Check if we have the object in the buffer + const std::string object_id = toHexString(object.object_id); + if (object_buffer_.count(object_id) == 0) { + return Maneuver::LANE_FOLLOW; + } + + // Object History Data + const std::deque object_info = object_buffer_.at(object_id); + + // Step2. Get the previous id + int prev_id = static_cast(object_info.size()) - 1; + while (prev_id >= 0) { + const double prev_time = rclcpp::Time(object_info.at(prev_id).pose.header.stamp).seconds(); + if (current_time - prev_time > history_time_length_) { + break; + } + --prev_id; + } + + if (prev_id < 0) { + return Maneuver::LANE_FOLLOW; + } + + // Step3. Get closest previous lanelet ID + const auto prev_pose = object_info.at(static_cast(prev_id)).pose; + const lanelet::ConstLanelets prev_lanelets = + object_info.at(static_cast(prev_id)).current_lanelets; + if (prev_lanelets.empty()) { + return Maneuver::LANE_FOLLOW; + } + lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); + double closest_prev_yaw = std::numeric_limits::max(); + for (const auto & lanelet : prev_lanelets) { + const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.pose.position); + const double delta_yaw = tf2::getYaw(prev_pose.pose.orientation) - lane_yaw; + const double normalized_delta_yaw = autoware_utils::normalizeRadian(delta_yaw); + if (normalized_delta_yaw < closest_prev_yaw) { + closest_prev_yaw = normalized_delta_yaw; + prev_lanelet = lanelet; + } + } + + // Step4. Check if the vehicle has changed lane + const auto current_pose = object.kinematics.pose_with_covariance.pose; + const double dist = autoware_utils::calcDistance2d(prev_pose, current_pose); + lanelet::routing::LaneletPaths possible_paths = + routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); + bool has_lane_changed = true; + for (const auto & path : possible_paths) { + for (const auto & lanelet : path) { + if (lanelet == current_lanelet) { + has_lane_changed = false; + break; + } + } + } + + if (has_lane_changed) { + return Maneuver::LANE_FOLLOW; + } + + // Step5. Lane Change Detection + const lanelet::ConstLineString2d prev_left_bound = prev_lanelet.leftBound2d(); + const lanelet::ConstLineString2d prev_right_bound = prev_lanelet.rightBound2d(); + const lanelet::ConstLineString2d current_left_bound = current_lanelet.leftBound2d(); + const lanelet::ConstLineString2d current_right_bound = current_lanelet.rightBound2d(); + const double prev_left_dist = calcLeftLateralOffset(prev_left_bound, prev_pose.pose); + const double prev_right_dist = calcRightLateralOffset(prev_right_bound, prev_pose.pose); + const double current_left_dist = calcLeftLateralOffset(current_left_bound, current_pose); + const double current_right_dist = calcRightLateralOffset(current_right_bound, current_pose); + const double prev_lane_width = std::fabs(prev_left_dist) + std::fabs(prev_right_dist); + const double current_lane_width = std::fabs(current_left_dist) + std::fabs(current_right_dist); + if (prev_lane_width < 1e-3 || current_lane_width < 1e-3) { + RCLCPP_ERROR(get_logger(), "[Map Based Prediction]: Lane Width is too small"); + return Maneuver::LANE_FOLLOW; + } + + const double current_left_dist_ratio = current_left_dist / current_lane_width; + const double current_right_dist_ratio = current_right_dist / current_lane_width; + const double diff_left_current_prev = current_left_dist - prev_left_dist; + const double diff_right_current_prev = current_right_dist - prev_right_dist; + + if ( + current_left_dist_ratio > dist_ratio_threshold_to_left_bound_ && + diff_left_current_prev > diff_dist_threshold_to_left_bound_) { + return Maneuver::LEFT_LANE_CHANGE; + } else if ( + current_right_dist_ratio < dist_ratio_threshold_to_right_bound_ && + diff_right_current_prev < diff_dist_threshold_to_right_bound_) { + return Maneuver::RIGHT_LANE_CHANGE; + } + + return Maneuver::LANE_FOLLOW; +} + +void MapBasedPredictionROS::objectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects) +{ + debug_accumulated_time_ = 0.0; + std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); + + if (!lanelet_map_ptr_) { + return; + } + + geometry_msgs::msg::TransformStamped world2map_transform; + geometry_msgs::msg::TransformStamped map2world_transform; + geometry_msgs::msg::TransformStamped debug_map2lidar_transform; + try { + world2map_transform = tf_buffer_ptr_->lookupTransform( + "map", // target + in_objects->header.frame_id, // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + map2world_transform = tf_buffer_ptr_->lookupTransform( + in_objects->header.frame_id, // target + "map", // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + debug_map2lidar_transform = tf_buffer_ptr_->lookupTransform( + "base_link", // target + "map", // src + rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), ex.what()); + return; + } + + /////////////////////////////////////////////////////////// + /////////////////// Update Object Buffer ////////////////// + ////////////////////////////////////////////////////////// + const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); + removeInvalidObject(objects_detected_time); + + ///////////////////////////////////////////////////////// + ///////////////////// Prediction /////////////////////// + /////////////////////////////////////////////////////// + autoware_auto_perception_msgs::msg::PredictedObjects objects_without_map; + objects_without_map.header = in_objects->header; + DynamicObjectWithLanesArray prediction_input; + prediction_input.header = in_objects->header; + + for (const auto & object : in_objects->objects) { + std::string object_id = toHexString(object.object_id); + DynamicObjectWithLanes transformed_object; + transformed_object.object = object; + if (in_objects->header.frame_id != "map") { + geometry_msgs::msg::PoseStamped pose_in_map; + geometry_msgs::msg::PoseStamped pose_orig; + pose_orig.pose = object.kinematics.pose_with_covariance.pose; + tf2::doTransform(pose_orig, pose_in_map, world2map_transform); + transformed_object.object.kinematics.pose_with_covariance.pose = pose_in_map.pose; + } + + std_msgs::msg::Header transformed_header = in_objects->header; + transformed_header.frame_id = "map"; + lanelet::ConstLanelets start_lanelets; // current lanelet + if (!updateObjectBuffer(transformed_header, transformed_object.object, start_lanelets)) { + objects_without_map.objects.push_back( + map_based_prediction_->convertToPredictedObject(transformed_object.object)); + continue; + } + + // Obtain valid Paths + const double delta_horizon = 1.0; + const double obj_vel = object.kinematics.twist_with_covariance.twist.linear.x; + lanelet::routing::LaneletPaths paths; + for (const auto & start_lanelet : start_lanelets) { + // Step1. Lane Change Detection + // First: Right to Left Detection Result + // Second: Left to Right Detection Result + const Maneuver maneuver = detectLaneChange(object, start_lanelet, objects_detected_time); + + // Step2. Get the left lanelet + lanelet::routing::LaneletPaths left_paths; + auto opt_left = routing_graph_ptr_->left(start_lanelet); + if (!!opt_left) { + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(*opt_left, search_dist, 0, false); + addValidPath(tmp_paths, left_paths); + } + } + + // Step3. Get the right lanelet + lanelet::routing::LaneletPaths right_paths; + auto opt_right = routing_graph_ptr_->right(start_lanelet); + if (!!opt_right) { + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(*opt_right, search_dist, 0, false); + addValidPath(tmp_paths, right_paths); + } + } + + // Step4. Get the centerline + lanelet::routing::LaneletPaths center_paths; + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(start_lanelet, search_dist, 0, false); + addValidPath(tmp_paths, center_paths); + } + + // Step5. Insert Valid Paths + if (!left_paths.empty() && maneuver == Maneuver::LEFT_LANE_CHANGE) { + paths.insert(paths.end(), left_paths.begin(), left_paths.end()); + } else if (!right_paths.empty() && maneuver == Maneuver::RIGHT_LANE_CHANGE) { + paths.insert(paths.end(), right_paths.begin(), right_paths.end()); + } else { + paths.insert(paths.end(), center_paths.begin(), center_paths.end()); + } + } + // If there is no valid path, we'll mark this object as map-less object + if (paths.empty()) { + objects_without_map.objects.push_back( + map_based_prediction_->convertToPredictedObject(transformed_object.object)); + continue; + } + + // Update Possible lanelet in the object buffer + updatePossibleLanelets(object_id, paths); + + std::vector> tmp_paths; + std::vector tmp_confidence; + for (const auto & path : paths) { + std::vector tmp_path; + + // Insert Positions. Note that we insert points from previous lanelet + if (!path.empty()) { + lanelet::ConstLanelets prev_lanelets = routing_graph_ptr_->previous(path.front()); + if (!prev_lanelets.empty()) { + lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); + for (const auto & point : prev_lanelet.centerline()) { + geometry_msgs::msg::Pose tmp_pose; + tmp_pose.position.x = point.x(); + tmp_pose.position.y = point.y(); + tmp_pose.position.z = point.z(); + tmp_path.push_back(tmp_pose); + } + } + } + + for (const auto & lanelet : path) { + for (const auto & point : lanelet.centerline()) { + geometry_msgs::msg::Pose tmp_pose; + tmp_pose.position.x = point.x(); + tmp_pose.position.y = point.y(); + tmp_pose.position.z = point.z(); + + // Prevent from inserting same points + if (!tmp_path.empty()) { + const auto prev_pose = tmp_path.back(); + const double tmp_dist = autoware_utils::calcDistance2d(prev_pose, tmp_pose); + if (tmp_dist < 1e-6) { + continue; + } + } + + tmp_path.push_back(tmp_pose); + } + } + + if (tmp_path.size() < 2) { + continue; + } + + // Compute yaw angles + for (size_t pose_id = 0; pose_id < tmp_path.size() - 1; ++pose_id) { + double tmp_yaw = std::atan2( + tmp_path.at(pose_id + 1).position.y - tmp_path.at(pose_id).position.y, + tmp_path.at(pose_id + 1).position.x - tmp_path.at(pose_id).position.x); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, tmp_yaw); + tmp_path.at(pose_id).orientation = tf2::toMsg(quat); + } + tmp_path.back().orientation = tmp_path.at(tmp_path.size() - 2).orientation; + + ////////////////////////////////////////////////////////////////////// + // Calculate Confidence of each path(centerline) for this obstacle // + //////////////////////////////////////////////////////////////////// + const double confidence = calculateLikelihood(tmp_path, transformed_object.object); + // Ignore a path that has too low confidence + if (confidence < 1e-6) { + continue; + } + + tmp_paths.push_back(tmp_path); + tmp_confidence.push_back(confidence); + } + + transformed_object.lanes = tmp_paths; + transformed_object.confidence = tmp_confidence; + prediction_input.objects.push_back(transformed_object); + } + + std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); + std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); + + std::vector out_objects_in_map; + map_based_prediction_->doPrediction(prediction_input, out_objects_in_map); + autoware_auto_perception_msgs::msg::PredictedObjects output; + output.header = in_objects->header; + output.header.frame_id = "map"; + output.objects = out_objects_in_map; + + std::vector out_objects_without_map; + map_based_prediction_->doLinearPrediction(objects_without_map, out_objects_without_map); + output.objects.insert( + output.objects.begin(), out_objects_without_map.begin(), out_objects_without_map.end()); + pub_objects_->publish(output); +} + +void MapBasedPredictionROS::mapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + RCLCPP_INFO(get_logger(), "Map is loaded"); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapBasedPredictionROS)