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 <[email protected]> * Fix build error for trajectory follower nodes Signed-off-by: wep21 <[email protected]> * [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 <[email protected]> * [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 <[email protected]> * move directory Signed-off-by: Takamasa Horibe <[email protected]> * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe <[email protected]> * remove .iv trajectory follower Signed-off-by: Takamasa Horibe <[email protected]> * use .auto trajectory_follower Signed-off-by: Takamasa Horibe <[email protected]> * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe <[email protected]> * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe <[email protected]> * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe <[email protected]> * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe <[email protected]> * apply .auto message split Signed-off-by: Takamasa Horibe <[email protected]> * fix build depend Signed-off-by: Takamasa Horibe <[email protected]> * 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 <[email protected]> Co-authored-by: Joshua Whitley <[email protected]> Co-authored-by: Igor Bogoslavskyi <[email protected]> Co-authored-by: MIURA Yasuyuki <[email protected]> Co-authored-by: wep21 <[email protected]> Co-authored-by: tomoya.kimura <[email protected]> * add isValidData (autowarefoundation#686) Signed-off-by: Takamasa Horibe <[email protected]> * Fix lane departure (autowarefoundation#688) * rename Signed-off-by: Takamasa Horibe <[email protected]> * fix bug Signed-off-by: Takamasa Horibe <[email protected]> * Update twist topic name (autowarefoundation#736) Signed-off-by: wep21 <[email protected]> * ci(pre-commit): autofix Co-authored-by: Takamasa Horibe <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]> Co-authored-by: Joshua Whitley <[email protected]> Co-authored-by: Igor Bogoslavskyi <[email protected]> Co-authored-by: MIURA Yasuyuki <[email protected]> Co-authored-by: wep21 <[email protected]> Co-authored-by: tomoya.kimura <[email protected]> Co-authored-by: Daisuke Nishimatsu <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takeshi Miura <[email protected]>
isamu-takagi · Dec 5, 2021 · 5d32d55 · 5d32d55
1 parent 7a041b1
commit 5d32d55
Show file tree

Hide file tree

Showing 10 changed files with 1,363 additions and 0 deletions.
diff --git a/control/lane_departure_checker/CMakeLists.txt 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
@@ -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
@@ -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
@@ -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_
diff --git a/...rol/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/...rol/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_