feat: add obstacle collision checker package (autowarefoundation#46)

* 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]> * Fix no ground pointcloud topic name (autowarefoundation#733) Signed-off-by: j4tfwm6z <[email protected]> Co-authored-by: j4tfwm6z <[email protected]> * Update twist topic name (autowarefoundation#736) Signed-off-by: wep21 <[email protected]> * fix/rename segmentation namespace (autowarefoundation#742) * rename segmentation directory * fix namespace: system stack * fix namespace: planning * fix namespace: control stack * fix namespace: perception stack * fix readme * 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: j4tfwm6z <[email protected]> Co-authored-by: Satoshi OTA <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
YoheiMishina · Dec 4, 2021 · f3a26bc · f3a26bc
1 parent ae73d48
commit f3a26bc
Show file tree

Hide file tree

Showing 10 changed files with 1,061 additions and 0 deletions.
diff --git a/control/obstacle_collision_checker/CMakeLists.txt b/control/obstacle_collision_checker/CMakeLists.txt
@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.5)
+project(obstacle_collision_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
+  ${PCL_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+# Target
+## obstacle_collision_checker_node
+ament_auto_add_library(obstacle_collision_checker SHARED
+  src/obstacle_collision_checker_node/obstacle_collision_checker.cpp
+  src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp
+)
+
+rclcpp_components_register_node(obstacle_collision_checker
+  PLUGIN "obstacle_collision_checker::ObstacleCollisionCheckerNode"
+  EXECUTABLE obstacle_collision_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/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md
@@ -0,0 +1,86 @@
+# obstacle_collision_checker
+
+## Purpose
+
+`obstacle_collision_checker` is a module to check obstacle collision for predicted trajectory and publish diagnostic errors if collision is found.
+
+## Inner-workings / Algorithms
+
+### Flow chart
+
+```plantuml
+@startuml
+skinparam monochrome true
+
+title obstacle collision checker : update
+start
+
+:calculate braking distance;
+
+:resampling trajectory;
+note right
+to reduce calculation cost
+end note
+:filter point cloud by trajectory;
+
+:create vehicle foot prints;
+
+:create vehicle passing area;
+
+partition will_collide {
+
+while (has next ego vehicle foot print) is (yes)
+  :found collision with obstacle foot print;
+  if (has collision with obstacle) then (yes)
+      :set diag to ERROR;
+      stop
+  endif
+end while (no)
+:set diag to OK;
+stop
+}
+
+@enduml
+```
+
+### Algorithms
+
+### Check data
+
+Check that `obstacle_collision_checker` receives no ground pointcloud, predicted_trajectory, reference trajectory, and current velocity data.
+
+### Diagnostic update
+
+If any collision is found on predicted path, this module sets `ERROR` level as diagnostic status else sets `OK`.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                           | Type                                           | Description                                                        |
+| ---------------------------------------------- | ---------------------------------------------- | ------------------------------------------------------------------ |
+| `~/input/trajectory`                           | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory                                               |
+| `~/input/trajectory`                           | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory                                               |
+| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2`                | Pointcloud of obstacles which the ego-vehicle should stop or avoid |
+| `/tf`                                          | `tf2_msgs::msg::TFMessage`                     | TF                                                                 |
+| `/tf_static`                                   | `tf2_msgs::msg::TFMessage`                     | TF static                                                          |
+
+### Output
+
+| Name             | Type                                   | Description              |
+| ---------------- | -------------------------------------- | ------------------------ |
+| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization |
+
+## Parameters
+
+| Name                | Type     | Description                                        | Default value |
+| :------------------ | :------- | :------------------------------------------------- | :------------ |
+| `delay_time`        | `double` | Delay time of vehicle [s]                          | 0.3           |
+| `footprint_margin`  | `double` | Foot print margin [m]                              | 0.0           |
+| `max_deceleration`  | `double` | Max deceleration for ego vehicle to stop [m/s^2]   | 2.0           |
+| `resample_interval` | `double` | Interval for resampling trajectory [m]             | 0.3           |
+| `search_radius`     | `double` | Search distance from trajectory to point cloud [m] | 5.0           |
+
+## Assumptions / Known limits
+
+To perform proper collision check, it is necessary to get probably predicted trajectory and obstacle pointclouds without noise.
diff --git a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml
@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    # Node
+    update_rate: 10.0
+
+    # Core
+    delay_time: 0.3 # delay time of vehicle [s]
+    footprint_margin: 0.0 # margin for footprint [m]
+    max_deceleration: 2.0 # max deceleration [m/ss]
+    resample_interval: 0.3 # interval distance to resample point cloud [m]
+    search_radius: 5.0 # search distance from trajectory to point cloud [m]
diff --git a/...tacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/...tacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp
@@ -0,0 +1,107 @@
+// Copyright 2020 Tier IV, Inc. 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 OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
+#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
+
+#include <autoware_utils/geometry/boost_geometry.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <boost/optional.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace obstacle_collision_checker
+{
+using autoware_utils::LinearRing2d;
+
+struct Param
+{
+  double delay_time;
+  double footprint_margin;
+  double max_deceleration;
+  double resample_interval;
+  double search_radius;
+};
+
+struct Input
+{
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose;
+  geometry_msgs::msg::Twist::ConstSharedPtr current_twist;
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud;
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory;
+};
+
+struct Output
+{
+  std::map<std::string, double> processing_time_map;
+  bool will_collide;
+  autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory;
+  std::vector<LinearRing2d> vehicle_footprints;
+  std::vector<LinearRing2d> vehicle_passing_areas;
+};
+
+class ObstacleCollisionChecker
+{
+public:
+  explicit ObstacleCollisionChecker(rclcpp::Node & node);
+  Output update(const Input & input);
+
+  void setParam(const Param & param) { param_ = param; }
+
+private:
+  Param param_;
+  vehicle_info_util::VehicleInfo vehicle_info_;
+
+  //! This function assumes the input trajectory is sampled dense enough
+  static autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
+    const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval);
+
+  static autoware_auto_planning_msgs::msg::Trajectory cutTrajectory(
+    const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length);
+
+  static std::vector<LinearRing2d> createVehicleFootprints(
+    const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param,
+    const vehicle_info_util::VehicleInfo & vehicle_info);
+
+  static std::vector<LinearRing2d> createVehiclePassingAreas(
+    const std::vector<LinearRing2d> & vehicle_footprints);
+
+  static LinearRing2d createHullFromFootprints(
+    const LinearRing2d & area1, const LinearRing2d & area2);
+
+  static bool willCollide(
+    const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
+    const std::vector<LinearRing2d> & vehicle_footprints);
+
+  static bool hasCollision(
+    const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
+    const LinearRing2d & vehicle_footprint);
+};
+}  // namespace obstacle_collision_checker
+
+#endif  // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
diff --git a/..._collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/..._collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp
@@ -0,0 +1,110 @@
+// Copyright 2020 Tier IV, Inc. 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 OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_
+#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_
+
+#include "obstacle_collision_checker/obstacle_collision_checker.hpp"
+
+#include <autoware_utils/geometry/geometry.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 <autoware_utils/ros/transform_listener.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <memory>
+#include <vector>
+
+namespace obstacle_collision_checker
+{
+struct NodeParam
+{
+  double update_rate;
+};
+
+class ObstacleCollisionCheckerNode : public rclcpp::Node
+{
+public:
+  explicit ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Subscriber
+  std::shared_ptr<autoware_utils::SelfPoseListener> self_pose_listener_;
+  std::shared_ptr<autoware_utils::TransformListener> transform_listener_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_obstacle_pointcloud_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    sub_reference_trajectory_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    sub_predicted_trajectory_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+
+  // Data Buffer
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
+  geometry_msgs::msg::Twist::ConstSharedPtr current_twist_;
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_;
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_;
+
+  // Callback
+  void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+  void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
+  void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
+  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+  // Publisher
+  std::shared_ptr<autoware_utils::DebugPublisher> debug_publisher_;
+  std::shared_ptr<autoware_utils::ProcessingTimePublisher> time_publisher_;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+  void initTimer(double period_s);
+
+  bool isDataReady();
+  bool isDataTimeout();
+  void onTimer();
+
+  // Parameter
+  NodeParam node_param_;
+  Param param_;
+
+  // Dynamic Reconfigure
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult paramCallback(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  // Core
+  Input input_;
+  Output output_;
+  std::unique_ptr<ObstacleCollisionChecker> obstacle_collision_checker_;
+
+  // Diagnostic Updater
+  diagnostic_updater::Updater updater_;
+
+  void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  // Visualization
+  visualization_msgs::msg::MarkerArray createMarkerArray() const;
+};
+}  // namespace obstacle_collision_checker
+
+#endif  // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_