feat: add lane depature checker package (autowarefoundation#44)

* Back port .auto control packages (autowarefoundation#571)

* Implement Lateral and Longitudinal Control Muxer

* [autowarefoundation#570] Porting wf_simulator

* [autowarefoundation#1189] Deactivate flaky test in 'trajectory_follower_nodes'

* [autowarefoundation#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer'

* [autowarefoundation#1057] Add osqp_interface package

* [autowarefoundation#1057] Add library code for MPC-based lateral control

* [autowarefoundation#1271] Use std::abs instead of abs

* [autowarefoundation#1057] Implement Lateral Controller for Cargo ODD

* [autowarefoundation#1246] Resolve "Test case names currently use snake_case but should be CamelCase"

* [autowarefoundation#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes'

* [autowarefoundation#1058] Add library code of longitudinal controller

* Fix build error for trajectory follower

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix build error for trajectory follower nodes

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [autowarefoundation#1272] Add AckermannControlCommand support to simple_planning_simulator

* [autowarefoundation#1058] Add Longitudinal Controller node

* [autowarefoundation#1058] Rename velocity_controller -> longitudinal_controller

* [autowarefoundation#1058] Update CMakeLists.txt for the longitudinal_controller_node

* [autowarefoundation#1058] Add smoke test python launch file

* [autowarefoundation#1058] Use LowPassFilter1d from trajectory_follower

* [autowarefoundation#1058] Use autoware_auto_msgs

* [autowarefoundation#1058] Changes for .auto (debug msg tmp fix, common func, tf listener)

* [autowarefoundation#1058] Remove unused parameters

* [autowarefoundation#1058] Fix ros test

* [autowarefoundation#1058] Rm default params from declare_parameters + use autoware types

* [autowarefoundation#1058] Use default param file to setup NodeOptions in the ros test

* [autowarefoundation#1058] Fix docstring

* [autowarefoundation#1058] Replace receiving a Twist with a VehicleKinematicState

* [autowarefoundation#1058] Change class variables format to m_ prefix

* [autowarefoundation#1058] Fix plugin name of LongitudinalController in CMakeLists.txt

* [autowarefoundation#1058] Fix copyright dates

* [autowarefoundation#1058] Reorder includes

* [autowarefoundation#1058] Add some tests (~89% coverage without disabling flaky tests)

* [autowarefoundation#1058] Add more tests (90+% coverage without disabling flaky tests)

* [autowarefoundation#1058] Use Float32MultiArrayDiagnostic message for debug and slope

* [autowarefoundation#1058] Calculate wheel_base value from vehicle parameters

* [autowarefoundation#1058] Cleanup redundant logger setting in tests

* [autowarefoundation#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures

* [autowarefoundation#1058] Remove TF listener and use published vehicle state instead

* [autowarefoundation#1058] Change smoke tests to use autoware_testing

* [autowarefoundation#1058] Add plotjuggler cfg for both lateral and longitudinal control

* [autowarefoundation#1058] Improve design documents

* [autowarefoundation#1058] Disable flaky test

* [autowarefoundation#1058] Properly transform vehicle state in longitudinal node

* [autowarefoundation#1058] Fix TF buffer of lateral controller

* [autowarefoundation#1058] Tuning of lateral controller for LGSVL

* [autowarefoundation#1058] Fix formating

* [autowarefoundation#1058] Fix /tf_static sub to be transient_local

* [autowarefoundation#1058] Fix yaw recalculation of reverse trajs in the lateral controller

* modify trajectory_follower for galactic build

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [autowarefoundation#1379] Update trajectory_follower

* [autowarefoundation#1379] Update simple_planning_simulator

* [autowarefoundation#1379] Update trajectory_follower_nodes

* apply trajectory msg modification in control

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move directory

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remote control/trajectory_follower level dorectpry

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove .iv trajectory follower

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use .auto trajectory_follower

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove .iv simple_planning_simulator & osqp_interface

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use .iv simple_planning_simulator & osqp_interface

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add tmp_autoware_auto_dependencies

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* tmporally add autoware_auto_msgs

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* apply .auto message split

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix build depend

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix packages using osqp

* fix autoware_auto_geometry

* ignore lint of some packages

* ignore ament_lint of some packages

* ignore lint/pre-commit of trajectory_follower_nodes

* disable unit tests of some packages

Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* add isValidData (autowarefoundation#686)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Fix lane departure (autowarefoundation#688)

* rename

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix bug

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update twist topic name (autowarefoundation#736)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* ci(pre-commit): autofix

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>

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
+)

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          |

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

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 <autoware_utils/geometry/boost_geometry.hpp>
+#include <autoware_utils/geometry/pose_deviation.hpp>
+#include <rosidl_runtime_cpp/message_initialization.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <boost/optional.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<TrajectoryPoint>;
+
+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<std::string, double> processing_time_map{};
+  bool will_leave_lane{};
+  bool is_out_of_lane{};
+  PoseDeviation trajectory_deviation{};
+  lanelet::ConstLanelets candidate_lanelets{};
+  TrajectoryPoints resampled_trajectory{};
+  std::vector<LinearRing2d> vehicle_footprints{};
+  std::vector<LinearRing2d> 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_util::VehicleInfo>(vehicle_info);
+  }
+
+  void setParam(const Param & param) { param_ = param; }
+
+private:
+  Param param_;
+  std::shared_ptr<vehicle_info_util::VehicleInfo> 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<LinearRing2d> createVehicleFootprints(
+    const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory,
+    const Param & param);
+
+  static std::vector<LinearRing2d> createVehiclePassingAreas(
+    const std::vector<LinearRing2d> & vehicle_footprints);
+
+  static bool willLeaveLane(
+    const lanelet::ConstLanelets & candidate_lanelets,
+    const std::vector<LinearRing2d> & 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_

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 <autoware_utils/ros/debug_publisher.hpp>
+#include <autoware_utils/ros/processing_time_publisher.hpp>
+#include <autoware_utils/ros/self_pose_listener.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_debug_msgs/msg/float64_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+
+#include <memory>
+#include <vector>
+
+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<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+  rclcpp::Subscription<HADMapBin>::SharedPtr sub_lanelet_map_bin_;
+  rclcpp::Subscription<HADMapRoute>::SharedPtr sub_route_;
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_reference_trajectory_;
+  rclcpp::Subscription<Trajectory>::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<rclcpp::Parameter> & parameters);
+
+  // Core
+  Input input_{};
+  Output output_{};
+  std::unique_ptr<LaneDepartureChecker> 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_

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 <Eigen/Dense>
+#include <autoware_utils/geometry/geometry.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <tf2/utils.h>
+
+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_

control/lane_departure_checker/launch/lane_departure_checker.launch.xml

@@ -0,0 +1,17 @@
+<launch>
+  <arg name="input/odometry" default="/localization/kinematic_state" />
+  <arg name="input/lanelet_map_bin" default="/map/vector_map" />
+  <arg name="input/route" default="/planning/mission_planning/route" />
+  <arg name="input/reference_trajectory" default="/planning/scenario_planning/trajectory" />
+  <arg name="input/predicted_trajectory" default="/control/trajectory_follower/predicted_trajectory" />
+  <arg name="config_file" default="$(find-pkg-share lane_departure_checker)/config/lane_departure_checker.param.yaml" />
+
+  <node pkg="lane_departure_checker" exec="lane_departure_checker_node" name="lane_departure_checker_node" output="screen">
+    <remap from="~/input/odometry" to="$(var input/odometry)"/>
+    <remap from="~/input/lanelet_map_bin" to="$(var input/lanelet_map_bin)"/>
+    <remap from="~/input/route" to="$(var input/route)"/>
+    <remap from="~/input/reference_trajectory" to="$(var input/reference_trajectory)"/>
+    <remap from="~/input/predicted_trajectory" to="$(var input/predicted_trajectory)"/>
+    <param from="$(var config_file)" />
+  </node>
+</launch>

control/lane_departure_checker/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lane_departure_checker</name>
+  <version>0.1.0</version>
+  <description>The lane_departure_checker package</description>
+  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_debug_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>boost</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>eigen</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>vehicle_info_util</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <autoware_utils/geometry/geometry.hpp>
+#include <autoware_utils/math/normalization.hpp>
+#include <autoware_utils/math/unit_conversion.hpp>
+#include <autoware_utils/system/stop_watch.hpp>
+
+#include <boost/geometry.hpp>
+
+#include <lanelet2_core/geometry/Polygon.h>
+#include <tf2/utils.h>
+
+#include <algorithm>
+#include <vector>
+
+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<TrajectoryPoint>;
+
+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<double> 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<size_t>(std::distance(distances.cbegin(), min_itr));
+
+  return min_idx;
+}
+
+LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & 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<LinearRing2d> & 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<std::chrono::milliseconds> 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<LinearRing2d> 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<LinearRing2d> 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<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
+  const std::vector<LinearRing2d> & vehicle_footprints)
+{
+  // Create hull from two adjacent vehicle footprints
+  std::vector<LinearRing2d> 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<LinearRing2d> & 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