Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: add lane depature checker package #44

45 changes: 45 additions & 0 deletions control/lane_departure_checker/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
)
80 changes: 80 additions & 0 deletions control/lane_departure_checker/README.md
Original file line number Diff line number Diff line change
@@ -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 |
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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_
Original file line number Diff line number Diff line change
@@ -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_
Loading