diff --git a/control/lane_departure_checker/CMakeLists.txt b/control/lane_departure_checker/CMakeLists.txt new file mode 100644 index 0000000000000..6cf1dc525d718 --- /dev/null +++ b/control/lane_departure_checker/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(lane_departure_checker) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Compile options +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 -Werror) +endif() + + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +# Target +## lane_departure_checker_node +ament_auto_add_library(lane_departure_checker SHARED + src/lane_departure_checker_node/lane_departure_checker.cpp + src/lane_departure_checker_node/lane_departure_checker_node.cpp +) + +rclcpp_components_register_node(lane_departure_checker + PLUGIN "lane_departure_checker::LaneDepartureCheckerNode" + EXECUTABLE lane_departure_checker_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md new file mode 100644 index 0000000000000..45ce9071140f4 --- /dev/null +++ b/control/lane_departure_checker/README.md @@ -0,0 +1,80 @@ +# Lane Departure Checker + +The **Lane Departure Checker** checks if vehicle follows a trajectory. If it does not follow the trajectory, it reports its status via `diagnostic_updater`. + +## Features + +This package includes the following features: + +- **Lane Departure**: Check if ego vehicle is going to be out of lane boundaries based on output from control module (predicted trajectory). +- **Trajectory Deviation**: Check if ego vehicle's pose does not deviate from the trajectory. Checking lateral, longitudinal and yaw deviation. + +## Inner-workings / Algorithms + +### How to extend footprint by covariance + +1. Calculate the standard deviation of error ellipse(covariance) in vehicle coordinate. + + 1.Transform covariance into vehicle coordinate. + + $$ + \begin{align} + \left( \begin{array}{cc} x_{vehicle}\\ y_{vehicle}\\ \end{array} \right) = R_{map2vehicle} \left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) + \end{align} + $$ + + Calculate covariance in vehicle coordinate. + + $$ + \begin{align} + Cov_{vehicle} &= E \left[ + \left( \begin{array}{cc} x_{vehicle}\\ y_{vehicle}\\ \end{array} \right) (x_{vehicle}, y_{vehicle}) \right] \\ + &= E \left[ R\left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) + (x_{map}, y_{map})R^t + \right] \\ + &= R E\left[ \left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) + (x_{map}, y_{map}) + \right] R^t \\ + &= R Cov_{map} R^t + \end{align} + $$ + + 2.The longitudinal length we want to expand is correspond to marginal distribution of $x_{vehicle}$, which is represented in $Cov_{vehicle}(0,0)$. In the same way, the lateral length is represented in $Cov_{vehicle}(1,1)$. Wikipedia reference [here](https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Marginal_distributions). + +2. Expand footprint based on the standard deviation multiplied with `footprint_margin_scale`. + +## Interface + +### Input + +- /localization/kinematic_state [`nav_msgs::msg::Odometry`] +- /map/vector_map [`autoware_auto_mapping_msgs::msg::HADMapBin`] +- /planning/mission_planning/route [`autoware_auto_planning_msgs::msg::HADMapRoute`] +- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] +- /control/trajectory_follower/predicted_trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] + +### Output + +- [`diagnostic_updater`] lane_departure : Update diagnostic level when ego vehicle is out of lane. +- [`diagnostic_updater`] trajectory_deviation : Update diagnostic level when ego vehicle deviates from trajectory. + +## Parameters + +### Node Parameters + +| Name | Type | Description | Default value | +| :---------------- | :----- | :---------------------------- | :------------ | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | + +### Core Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | +| footprint_margin_scale | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation. | 1.0 | +| resample_interval | double | Minimum Euclidean distance between points when resample trajectory.[m] | 0.3 | +| max_deceleration | double | Maximum deceleration when calculating braking distance. | 2.8 | +| delay_time | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3 | +| max_lateral_deviation | double | Maximum lateral deviation in vehicle coordinate. [m] | 2.0 | +| max_longitudinal_deviation | double | Maximum longitudinal deviation in vehicle coordinate. [m] | 2.0 | +| max_yaw_deviation_deg | double | Maximum ego yaw deviation from trajectory. [deg] | 60.0 | diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml new file mode 100644 index 0000000000000..f4925fdf17ec4 --- /dev/null +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + # Node + update_rate: 10.0 + visualize_lanelet: false + + # Core + footprint_margin_scale: 1.0 + resample_interval: 0.3 + max_deceleration: 2.8 + delay_time: 1.3 + max_lateral_deviation: 2.0 + max_longitudinal_deviation: 2.0 + max_yaw_deviation_deg: 60.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp new file mode 100644 index 0000000000000..1dd6fe3558d82 --- /dev/null +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -0,0 +1,124 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace lane_departure_checker +{ +using autoware_auto_planning_msgs::msg::HADMapRoute; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::LinearRing2d; +using autoware_utils::PoseDeviation; +using TrajectoryPoints = std::vector; + +struct Param +{ + double footprint_margin_scale; + double resample_interval; + double max_deceleration; + double delay_time; + double max_lateral_deviation; + double max_longitudinal_deviation; + double max_yaw_deviation_deg; +}; + +struct Input +{ + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose{}; + nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; + lanelet::LaneletMapPtr lanelet_map{}; + HADMapRoute::ConstSharedPtr route{}; + lanelet::ConstLanelets route_lanelets{}; + Trajectory::ConstSharedPtr reference_trajectory{}; + Trajectory::ConstSharedPtr predicted_trajectory{}; +}; + +struct Output +{ + std::map processing_time_map{}; + bool will_leave_lane{}; + bool is_out_of_lane{}; + PoseDeviation trajectory_deviation{}; + lanelet::ConstLanelets candidate_lanelets{}; + TrajectoryPoints resampled_trajectory{}; + std::vector vehicle_footprints{}; + std::vector vehicle_passing_areas{}; +}; + +class LaneDepartureChecker +{ +public: + Output update(const Input & input); + + void setParam(const Param & param, const vehicle_info_util::VehicleInfo vehicle_info) + { + param_ = param; + vehicle_info_ptr_ = std::make_shared(vehicle_info); + } + + void setParam(const Param & param) { param_ = param; } + +private: + Param param_; + std::shared_ptr vehicle_info_ptr_; + + static PoseDeviation calcTrajectoryDeviation( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose); + + //! This function assumes the input trajectory is sampled dense enough + static TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double interval); + + static TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double length); + + std::vector createVehicleFootprints( + const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, + const Param & param); + + static std::vector createVehiclePassingAreas( + const std::vector & vehicle_footprints); + + static bool willLeaveLane( + const lanelet::ConstLanelets & candidate_lanelets, + const std::vector & vehicle_footprints); + + static bool isOutOfLane( + const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint); +}; +} // namespace lane_departure_checker + +#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp new file mode 100644 index 0000000000000..ac679279f6f2f --- /dev/null +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -0,0 +1,124 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ + +#include "lane_departure_checker/lane_departure_checker.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace lane_departure_checker +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; + +struct NodeParam +{ + double update_rate; + bool visualize_lanelet; +}; + +class LaneDepartureCheckerNode : public rclcpp::Node +{ +public: + explicit LaneDepartureCheckerNode(const rclcpp::NodeOptions & options); + +private: + // Subscriber + autoware_utils::SelfPoseListener self_pose_listener_{this}; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; + + // Data Buffer + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; + lanelet::LaneletMapPtr lanelet_map_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_; + lanelet::routing::RoutingGraphPtr routing_graph_; + HADMapRoute::ConstSharedPtr route_; + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr cov_; + HADMapRoute::ConstSharedPtr last_route_; + lanelet::ConstLanelets route_lanelets_; + Trajectory::ConstSharedPtr reference_trajectory_; + Trajectory::ConstSharedPtr predicted_trajectory_; + + // Callback + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onLaneletMapBin(const HADMapBin::ConstSharedPtr msg); + void onRoute(const HADMapRoute::ConstSharedPtr msg); + void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); + void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); + + // Publisher + autoware_utils::DebugPublisher debug_publisher_{this, "~/debug"}; + autoware_utils::ProcessingTimePublisher processing_time_publisher_{this}; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + bool isDataReady(); + bool isDataTimeout(); + bool isDataValid(); + void onTimer(); + + // Parameter + NodeParam node_param_; + Param param_; + double vehicle_length_m_; + + // Parameter callback + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + // Core + Input input_{}; + Output output_{}; + std::unique_ptr lane_departure_checker_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_{this}; + + void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat); + void checkTrajectoryDeviation(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Visualization + visualization_msgs::msg::MarkerArray createMarkerArray() const; +}; +} // namespace lane_departure_checker + +#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp new file mode 100644 index 0000000000000..2bcaebd5341bc --- /dev/null +++ b/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp @@ -0,0 +1,92 @@ +// 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. +// Copyright 2015-2020 Autoware Foundation. All rights reserved. +// +// 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 LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ + +#include +#include +#include + +#include + +struct FootprintMargin +{ + double lon; + double lat; +}; + +inline autoware_utils::LinearRing2d createVehicleFootprint( + const vehicle_info_util::VehicleInfo & vehicle_info, const FootprintMargin & margin = {0.0, 0.0}) +{ + using autoware_utils::LinearRing2d; + using autoware_utils::Point2d; + + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m + i.wheel_base_m + margin.lon; + const double x_center = i.wheel_base_m / 2.0; + const double x_rear = -(i.rear_overhang_m + margin.lon); + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin.lat; + const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin.lat); + + LinearRing2d footprint; + footprint.push_back(Point2d{x_front, y_left}); + footprint.push_back(Point2d{x_front, y_right}); + footprint.push_back(Point2d{x_center, y_right}); + footprint.push_back(Point2d{x_rear, y_right}); + footprint.push_back(Point2d{x_rear, y_left}); + footprint.push_back(Point2d{x_center, y_left}); + footprint.push_back(Point2d{x_front, y_left}); + + return footprint; +} + +inline FootprintMargin calcFootprintMargin( + const geometry_msgs::msg::PoseWithCovariance & covariance, const double scale) +{ + const auto Cov_in_map = covariance.covariance; + Eigen::Matrix2d Cov_xy_map; + Cov_xy_map << Cov_in_map[0 * 6 + 0], Cov_in_map[0 * 6 + 1], Cov_in_map[1 * 6 + 0], + Cov_in_map[1 * 6 + 1]; + + const double yaw_vehicle = tf2::getYaw(covariance.pose.orientation); + + // To get a position in a transformed coordinate, rotate the inverse direction + Eigen::Matrix2d R_map2vehicle; + R_map2vehicle << std::cos(-yaw_vehicle), -std::sin(-yaw_vehicle), std::sin(-yaw_vehicle), + std::cos(-yaw_vehicle); + // Rotate covariance E((X, Y)^t*(X, Y)) = E(R*(x,y)*(x,y)^t*R^t) + // when Rotate point (X, Y)^t= R*(x, y)^t. + const Eigen::Matrix2d Cov_xy_vehicle = R_map2vehicle * Cov_xy_map * R_map2vehicle.transpose(); + + // The longitudinal/lateral length is represented + // in Cov_xy_vehicle(0,0), Cov_xy_vehicle(1,1) respectively. + return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; +} + +#endif // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml new file mode 100644 index 0000000000000..29db32f53e56e --- /dev/null +++ b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml new file mode 100644 index 0000000000000..2072f89a728dc --- /dev/null +++ b/control/lane_departure_checker/package.xml @@ -0,0 +1,35 @@ + + + lane_departure_checker + 0.1.0 + The lane_departure_checker package + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_mapping_msgs + autoware_auto_planning_msgs + autoware_debug_msgs + autoware_utils + boost + diagnostic_updater + eigen + geometry_msgs + lanelet2_extension + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp new file mode 100644 index 0000000000000..6031296c79a25 --- /dev/null +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -0,0 +1,282 @@ +// 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 "lane_departure_checker/lane_departure_checker.hpp" + +#include "lane_departure_checker/util/create_vehicle_footprint.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +using autoware_utils::LinearRing2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::Point2d; + +namespace +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +double calcBrakingDistance( + const double abs_velocity, const double max_deceleration, const double delay_time) +{ + return (abs_velocity * abs_velocity) / (2.0 * max_deceleration) + delay_time * abs_velocity; +} + +bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2d & point) +{ + for (const auto & ll : candidate_lanelets) { + if (boost::geometry::within(point, ll.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +size_t findNearestIndex(const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose) +{ + std::vector distances; + distances.reserve(trajectory.points.size()); + std::transform( + trajectory.points.cbegin(), trajectory.points.cend(), std::back_inserter(distances), + [&](const TrajectoryPoint & p) { + const auto p1 = autoware_utils::fromMsg(p.pose.position).to_2d(); + const auto p2 = autoware_utils::fromMsg(pose.position).to_2d(); + return boost::geometry::distance(p1, p2); + }); + + const auto min_itr = std::min_element(distances.cbegin(), distances.cend()); + const auto min_idx = static_cast(std::distance(distances.cbegin(), min_itr)); + + return min_idx; +} + +LinearRing2d createHullFromFootprints(const std::vector & footprints) +{ + MultiPoint2d combined; + for (const auto & footprint : footprints) { + for (const auto & p : footprint) { + combined.push_back(p); + } + } + + LinearRing2d hull; + boost::geometry::convex_hull(combined, hull); + + return hull; +} + +lanelet::ConstLanelets getCandidateLanelets( + const lanelet::ConstLanelets & route_lanelets, + const std::vector & vehicle_footprints) +{ + lanelet::ConstLanelets candidate_lanelets; + + // Find lanes within the convex hull of footprints + const auto footprint_hull = createHullFromFootprints(vehicle_footprints); + for (const auto & route_lanelet : route_lanelets) { + const auto poly = route_lanelet.polygon2d().basicPolygon(); + if (!boost::geometry::disjoint(poly, footprint_hull)) { + candidate_lanelets.push_back(route_lanelet); + } + } + + return candidate_lanelets; +} +} // namespace + +namespace lane_departure_checker +{ +Output LaneDepartureChecker::update(const Input & input) +{ + Output output{}; + + autoware_utils::StopWatch stop_watch; + + output.trajectory_deviation = + calcTrajectoryDeviation(*input.reference_trajectory, input.current_pose->pose); + output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true); + + { + constexpr double min_velocity = 0.01; + const auto & raw_abs_velocity = std::abs(input.current_odom->twist.twist.linear.x); + const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity; + + const auto braking_distance = + calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time); + + output.resampled_trajectory = cutTrajectory( + resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance); + output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true); + } + output.vehicle_footprints = + createVehicleFootprints(input.current_odom->pose, output.resampled_trajectory, param_); + output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); + + output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); + output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); + + output.candidate_lanelets = getCandidateLanelets(input.route_lanelets, output.vehicle_footprints); + output.processing_time_map["getCandidateLanelets"] = stop_watch.toc(true); + + output.will_leave_lane = willLeaveLane(output.candidate_lanelets, output.vehicle_footprints); + output.processing_time_map["willLeaveLane"] = stop_watch.toc(true); + + output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); + output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); + + return output; +} + +PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose) +{ + const auto nearest_idx = findNearestIndex(trajectory, pose); + return autoware_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); +} + +TrajectoryPoints LaneDepartureChecker::resampleTrajectory( + const Trajectory & trajectory, const double interval) +{ + TrajectoryPoints resampled; + + resampled.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = autoware_utils::fromMsg(resampled.back().pose.position); + const auto p2 = autoware_utils::fromMsg(point.pose.position); + + if (boost::geometry::distance(p1.to_2d(), p2.to_2d()) > interval) { + resampled.push_back(point); + } + } + resampled.push_back(trajectory.points.back()); + + return resampled; +} + +TrajectoryPoints LaneDepartureChecker::cutTrajectory( + const TrajectoryPoints & trajectory, const double length) +{ + TrajectoryPoints cut; + + double total_length = 0.0; + cut.push_back(trajectory.front()); + for (size_t i = 1; i < trajectory.size(); ++i) { + const auto & point = trajectory.at(i); + + const auto p1 = autoware_utils::fromMsg(cut.back().pose.position); + const auto p2 = autoware_utils::fromMsg(point.pose.position); + const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + + const auto remain_distance = length - total_length; + + // Over length + if (remain_distance <= 0) { + break; + } + + // Require interpolation + if (remain_distance <= points_distance) { + const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); + + TrajectoryPoint p; + p.pose.position.x = p_interpolated.x(); + p.pose.position.y = p_interpolated.y(); + p.pose.position.z = p_interpolated.z(); + p.pose.orientation = point.pose.orientation; + + cut.push_back(p); + break; + } + + cut.push_back(point); + total_length += points_distance; + } + + return cut; +} + +std::vector LaneDepartureChecker::createVehicleFootprints( + const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, + const Param & param) +{ + // Calculate longitudinal and lateral margin based on covariance + const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale); + + // Create vehicle footprint in base_link coordinate + const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_, margin); + + // Create vehicle footprint on each TrajectoryPoint + std::vector vehicle_footprints; + for (const auto & p : trajectory) { + vehicle_footprints.push_back( + transformVector(local_vehicle_footprint, autoware_utils::pose2transform(p.pose))); + } + + return vehicle_footprints; +} + +std::vector LaneDepartureChecker::createVehiclePassingAreas( + const std::vector & vehicle_footprints) +{ + // Create hull from two adjacent vehicle footprints + std::vector areas; + for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) { + const auto & footprint1 = vehicle_footprints.at(i); + const auto & footprint2 = vehicle_footprints.at(i + 1); + areas.push_back(createHullFromFootprints({footprint1, footprint2})); + } + + return areas; +} + +bool LaneDepartureChecker::willLeaveLane( + const lanelet::ConstLanelets & candidate_lanelets, + const std::vector & vehicle_footprints) +{ + for (const auto & vehicle_footprint : vehicle_footprints) { + if (isOutOfLane(candidate_lanelets, vehicle_footprint)) { + return true; + } + } + + return false; +} + +bool LaneDepartureChecker::isOutOfLane( + const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint) +{ + for (const auto & point : vehicle_footprint) { + if (!isInAnyLane(candidate_lanelets, point)) { + return true; + } + } + + return false; +} +} // namespace lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp new file mode 100644 index 0000000000000..12638bdaa1f2d --- /dev/null +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -0,0 +1,550 @@ +// 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 "lane_departure_checker/lane_departure_checker_node.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +using autoware_utils::rad2deg; + +namespace +{ +using autoware_auto_mapping_msgs::msg::HADMapSegment; + +std::array triangle2points( + const geometry_msgs::msg::Polygon & triangle) +{ + std::array points; + for (size_t i = 0; i < 3; ++i) { + const auto & p = triangle.points.at(i); + + geometry_msgs::msg::Point point; + point.x = static_cast(p.x); + point.y = static_cast(p.y); + point.z = static_cast(p.z); + points.at(i) = point; + } + return points; +} + +lanelet::ConstLanelets getRouteLanelets( + const lanelet::LaneletMap & lanelet_map, const lanelet::routing::RoutingGraphPtr & routing_graph, + const std::vector & route_sections, const double vehicle_length) +{ + lanelet::ConstLanelets route_lanelets; + + // Add preceding lanes of front route_section to prevent detection errors + { + const auto extension_length = 2 * vehicle_length; + + for (const auto & primitive : route_sections.front().primitives) { + const auto lane_id = primitive.id; + for (const auto & lanelet_sequence : lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph, lanelet_map.laneletLayer.get(lane_id), extension_length)) { + for (const auto & preceding_lanelet : lanelet_sequence) { + route_lanelets.push_back(preceding_lanelet); + } + } + } + } + + for (const auto & route_section : route_sections) { + for (const auto & primitive : route_section.primitives) { + const auto lane_id = primitive.id; + route_lanelets.push_back(lanelet_map.laneletLayer.get(lane_id)); + } + } + + // Add succeeding lanes of last route_section to prevent detection errors + { + const auto extension_length = 2 * vehicle_length; + + for (const auto & primitive : route_sections.back().primitives) { + const auto lane_id = primitive.id; + for (const auto & lanelet_sequence : lanelet::utils::query::getSucceedingLaneletSequences( + routing_graph, lanelet_map.laneletLayer.get(lane_id), extension_length)) { + for (const auto & succeeding_lanelet : lanelet_sequence) { + route_lanelets.push_back(succeeding_lanelet); + } + } + } + } + + return route_lanelets; +} + +template +void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + value = it->template get_value(); + } +} + +} // namespace + +namespace lane_departure_checker +{ +LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & options) +: Node("lane_departure_checker_node", options) +{ + using std::placeholders::_1; + + // Node Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + node_param_.visualize_lanelet = declare_parameter("visualize_lanelet", false); + + // Core Parameter + + // Vehicle Info + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_length_m_ = vehicle_info.vehicle_length_m; + + param_.footprint_margin_scale = declare_parameter("footprint_margin_scale", 1.0); + param_.resample_interval = declare_parameter("resample_interval", 0.3); + param_.max_deceleration = declare_parameter("max_deceleration", 3.0); + param_.delay_time = declare_parameter("delay_time", 0.3); + param_.max_lateral_deviation = declare_parameter("max_lateral_deviation", 1.0); + param_.max_longitudinal_deviation = declare_parameter("max_longitudinal_deviation", 1.0); + param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 30.0); + + // Parameter Callback + set_param_res_ = + add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1)); + + // Core + lane_departure_checker_ = std::make_unique(); + lane_departure_checker_->setParam(param_, vehicle_info); + + // Subscriber + sub_odom_ = this->create_subscription( + "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); + sub_lanelet_map_bin_ = this->create_subscription( + "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), + std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); + sub_route_ = this->create_subscription( + "~/input/route", 1, std::bind(&LaneDepartureCheckerNode::onRoute, this, _1)); + sub_reference_trajectory_ = this->create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&LaneDepartureCheckerNode::onReferenceTrajectory, this, _1)); + sub_predicted_trajectory_ = this->create_subscription( + "~/input/predicted_trajectory", 1, + std::bind(&LaneDepartureCheckerNode::onPredictedTrajectory, this, _1)); + + // Publisher + // Nothing + + // Diagnostic Updater + updater_.setHardwareID("lane_departure_checker"); + + updater_.add("lane_departure", this, &LaneDepartureCheckerNode::checkLaneDeparture); + + updater_.add("trajectory_deviation", this, &LaneDepartureCheckerNode::checkTrajectoryDeviation); + + // Wait for first self pose + self_pose_listener_.waitForFirstPose(); + + // Timer + double delta_time = 1.0 / static_cast(node_param_.update_rate); + auto timer_callback_ = std::bind(&LaneDepartureCheckerNode::onTimer, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(delta_time)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback_), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + current_odom_ = msg; +} + +void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr msg) +{ + lanelet_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); +} + +void LaneDepartureCheckerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) { route_ = msg; } + +void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) +{ + reference_trajectory_ = msg; +} + +void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) +{ + predicted_trajectory_ = msg; +} + +bool LaneDepartureCheckerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_pose..."); + return false; + } + + if (!current_odom_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_twist msg..."); + return false; + } + + if (!lanelet_map_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for lanelet_map msg..."); + return false; + } + + if (!route_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route msg..."); + return false; + } + + if (!reference_trajectory_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for reference_trajectory msg..."); + return false; + } + + if (!predicted_trajectory_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for predicted_trajectory msg..."); + return false; + } + + return true; +} + +bool LaneDepartureCheckerNode::isDataTimeout() +{ + const auto now = this->now(); + + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp) - now; + if (pose_time_diff.seconds() > th_pose_timeout) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "pose is timeout..."); + return true; + } + + return false; +} + +bool LaneDepartureCheckerNode::isDataValid() +{ + if (reference_trajectory_->points.empty()) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, "reference_trajectory is empty. Not expected!"); + return false; + } + + if (predicted_trajectory_->points.empty()) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, "predicted_trajectory is empty. Not expected!"); + return false; + } + + return true; +} + +void LaneDepartureCheckerNode::onTimer() +{ + std::map processing_time_map; + autoware_utils::StopWatch stop_watch; + stop_watch.tic("Total"); + + current_pose_ = self_pose_listener_.getCurrentPose(); + + if (!isDataReady()) { + return; + } + + if (isDataTimeout()) { + return; + } + + if (!isDataValid()) { + return; + } + + processing_time_map["Node: checkData"] = stop_watch.toc(true); + + // In order to wait for both of map and route will be ready, write this not in callback but here + if (last_route_ != route_) { + route_lanelets_ = + getRouteLanelets(*lanelet_map_, routing_graph_, route_->segments, vehicle_length_m_); + last_route_ = route_; + } + processing_time_map["Node: getRouteLanelets"] = stop_watch.toc(true); + + input_.current_odom = current_odom_; + input_.current_pose = current_pose_; + input_.lanelet_map = lanelet_map_; + input_.route = route_; + input_.route_lanelets = route_lanelets_; + input_.reference_trajectory = reference_trajectory_; + input_.predicted_trajectory = predicted_trajectory_; + processing_time_map["Node: setInputData"] = stop_watch.toc(true); + + output_ = lane_departure_checker_->update(input_); + processing_time_map["Node: update"] = stop_watch.toc(true); + + updater_.force_update(); + processing_time_map["Node: updateDiagnostics"] = stop_watch.toc(true); + + { + const auto & deviation = output_.trajectory_deviation; + debug_publisher_.publish( + "deviation/lateral", deviation.lateral); + debug_publisher_.publish( + "deviation/yaw", deviation.yaw); + debug_publisher_.publish( + "deviation/yaw_deg", rad2deg(deviation.yaw)); + } + processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); + + debug_publisher_.publish( + std::string("marker_array"), createMarkerArray()); + processing_time_map["Node: publishDebugMarker"] = stop_watch.toc(true); + + // Merge processing_time_map + for (const auto & m : output_.processing_time_map) { + processing_time_map["Core: " + m.first] = m.second; + } + + processing_time_map["Total"] = stop_watch.toc("Total"); + processing_time_publisher_.publish(processing_time_map); +} + +rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + try { + // Node + update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet); + + // Core + update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); + update_param(parameters, "resample_interval", param_.resample_interval); + update_param(parameters, "max_deceleration", param_.max_deceleration); + update_param(parameters, "delay_time", param_.delay_time); + + if (lane_departure_checker_) { + lane_departure_checker_->setParam(param_); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + + return result; +} + +void LaneDepartureCheckerNode::checkLaneDeparture( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; + int8_t level = DiagStatus::OK; + std::string msg = "OK"; + + if (output_.will_leave_lane) { + level = DiagStatus::WARN; + msg = "vehicle will leave lane"; + } + + if (output_.is_out_of_lane) { + level = DiagStatus::ERROR; + msg = "vehicle is out of lane"; + } + + stat.summary(level, msg); +} + +void LaneDepartureCheckerNode::checkTrajectoryDeviation( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; + int8_t level = DiagStatus::OK; + + if (std::abs(output_.trajectory_deviation.lateral) >= param_.max_lateral_deviation) { + level = DiagStatus::ERROR; + } + + if (std::abs(output_.trajectory_deviation.longitudinal) >= param_.max_longitudinal_deviation) { + level = DiagStatus::ERROR; + } + + if (std::abs(rad2deg(output_.trajectory_deviation.yaw)) >= param_.max_yaw_deviation_deg) { + level = DiagStatus::ERROR; + } + + std::string msg = "OK"; + if (level == DiagStatus::ERROR) { + msg = "trajectory deviation is too large"; + } + + stat.addf("max lateral deviation", "%.3f", param_.max_lateral_deviation); + stat.addf("lateral deviation", "%.3f", output_.trajectory_deviation.lateral); + + stat.addf("max longitudinal deviation", "%.3f", param_.max_longitudinal_deviation); + stat.addf("longitudinal deviation", "%.3f", output_.trajectory_deviation.longitudinal); + + stat.addf("max yaw deviation", "%.3f", param_.max_yaw_deviation_deg); + stat.addf("yaw deviation", "%.3f", rad2deg(output_.trajectory_deviation.yaw)); + + stat.summary(level, msg); +} + +visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const +{ + using autoware_utils::createDefaultMarker; + using autoware_utils::createMarkerColor; + using autoware_utils::createMarkerScale; + + visualization_msgs::msg::MarkerArray marker_array; + + const auto base_link_z = current_pose_->pose.position.z; + + if (node_param_.visualize_lanelet) { + // Route Lanelets + { + auto marker = createDefaultMarker( + "map", this->now(), "route_lanelets", 0, visualization_msgs::msg::Marker::TRIANGLE_LIST, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(0.0, 0.5, 0.5, 0.5)); + + for (const auto & lanelet : input_.route_lanelets) { + std::vector triangles; + lanelet::visualization::lanelet2Triangle(lanelet, &triangles); + + for (const auto & triangle : triangles) { + for (const auto & point : triangle2points(triangle)) { + marker.points.push_back(point); + marker.colors.push_back(marker.color); + } + } + } + + marker_array.markers.push_back(marker); + } + + // Candidate Lanelets + { + auto marker = createDefaultMarker( + "map", this->now(), "candidate_lanelets", 0, visualization_msgs::msg::Marker::TRIANGLE_LIST, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.5)); + + for (const auto & lanelet : output_.candidate_lanelets) { + std::vector triangles; + lanelet::visualization::lanelet2Triangle(lanelet, &triangles); + + for (const auto & triangle : triangles) { + for (const auto & point : triangle2points(triangle)) { + marker.points.push_back(point); + marker.colors.push_back(marker.color); + } + } + } + + marker_array.markers.push_back(marker); + } + } + + if (output_.resampled_trajectory.size() >= 2) { + // Line of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", this->now(), "resampled_trajectory_line", 0, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.05, 0, 0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + for (const auto & p : output_.resampled_trajectory) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + + // Points of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", this->now(), "resampled_trajectory_points", 0, + visualization_msgs::msg::Marker::SPHERE_LIST, createMarkerScale(0.1, 0.1, 0.1), + createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & p : output_.resampled_trajectory) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + } + + // Vehicle Footprints + { + const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); + const auto color_will_leave_lane = createMarkerColor(0.5, 0.5, 0.0, 0.5); + const auto color_is_out_of_lane = createMarkerColor(1.0, 0.0, 0.0, 0.5); + + auto color = color_ok; + if (output_.will_leave_lane) { + color = color_will_leave_lane; + } + if (output_.is_out_of_lane) { + color = color_is_out_of_lane; + } + + auto marker = createDefaultMarker( + "map", this->now(), "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0, 0), color); + + for (const auto & vehicle_footprint : output_.vehicle_footprints) { + for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { + const auto p1 = vehicle_footprint.at(i); + const auto p2 = vehicle_footprint.at(i + 1); + + marker.points.push_back(toMsg(p1.to_3d(base_link_z))); + marker.points.push_back(toMsg(p2.to_3d(base_link_z))); + } + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} +} // namespace lane_departure_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(lane_departure_checker::LaneDepartureCheckerNode)