diff --git a/README.md b/README.md index 1d81e2e..6eeeffb 100644 --- a/README.md +++ b/README.md @@ -120,26 +120,32 @@ sudo apt install libvulkan1 - [git lfs](https://packagecloud.io/github/git-lfs/install) - [ROS2](https://docs.ros.org/en/galactic/index.html)(動作確認済みバージョン:Galactic) #### **Dockerイメージの準備・起動 〜 Autowareの準備** + +##### **Dockerイメージ(v3.1)について(2023/1/6追記)** +Dockerイメージは[autoware(fc50327ec926d5c9a04d385581f102a418af0403)](https://github.com/autowarefoundation/autoware/commit/fc50327ec926d5c9a04d385581f102a418af0403)に以下を適用しています。 +- [fix(pointcloud_preprocessor): add missed target dependency #2101](https://github.com/autowarefoundation/autoware.universe/pull/2101/files)の修正を適用して提供しています。 +- tier4_*_launchパッケージの削除 + - aichallenge_submitに移動しています。autowareのlaunch, configファイルを変更したい場合はこちらを編集してください。 + 1. Dockerイメージを入手 ``` -docker pull ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:latest +docker pull ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:3.1 ``` ※上記の方法では長時間かかってしまう方・タイムアウトしてしまう方↓ -[こちら](https://drive.google.com/drive/u/2/folders/1VZAcGzcFpOBJlmmybcGau7BaHzZW5Chc)に、イメージをtarにまとめたものを置きましたので、下記コマンドよりご利用ください。 +[こちら](https://drive.google.com/file/d/145HyoeXye_bbdT6tOVVCvbSM1MTm2CKI/view?usp=share_link)に、イメージをtarにまとめたものを置きましたので、下記コマンドよりご利用ください。 ``` -gzip -d aichallenge2022_sim_autoware_v2.0.tar.gz -docker load < aichallenge2022_sim_autoware_v2.0.tar +docker load < aichallenge2022_sim_autoware_v3.1.tar.gz ``` -2. 大会用データのダウンロード +1. 大会用データのダウンロード ``` sudo apt install -y git-lfs git lfs clone https://github.com/AutomotiveAIChallenge/aichallenge2022-sim ``` -3. rockerを起動 +1. rockerを起動 ``` cd ./aichallenge2022-sim -rocker --nvidia --x11 --user --net host --privileged --volume autoware:/aichallenge -- ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:latest +rocker --nvidia --x11 --user --net host --privileged --volume autoware:/aichallenge -- ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:3.1 ``` ### **サンプルコード(ROS2パッケージ)** @@ -159,6 +165,11 @@ rocker --nvidia --x11 --user --net host --privileged --volume autoware:/aichalle - `aichallenge_submit_launch.launch.xml`が大元のlaunchファイル`aichallenge.launch.xml`から呼び出されますので、このlaunchファイルを適宜改修して皆様が実装されたROS2ノードが起動されるように設定してください。 - sample_code_cpp - サンプルの自動走行実装です。 + - obstacle_stop_planner_custom + - autoware.universeのobstacle_stop_plannerから障害物を誤検出する問題を解消しています。 + - tier4_*_launch + - autowareのlaunchファイルをコピーして一部編集したものです。autowareのtier4_*_launchは削除しているため、こちらを必ずaichallenge_submit内に残すようにしてください。 + - obstacle_stop_plannerの代わりにobstacle_stop_planner_customを呼び出すように変更してあります。 ### **サンプルコードビルド** ``` @@ -218,6 +229,75 @@ ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehic ## タイム取得 タイム取得方法については[RULE.md](/RULE.md)を参照ください。 + +## オンライン評価環境について +### 評価時のオンライン環境での実行フローの概略 +スコアの算出にあたっては、オンライン評価環境のwebページよりパッケージ`aichallenge_submit`のみを提出していただき、自動採点を行います。 +提出後、オンライン評価環境では`evaluation/`以下のスクリプトを使って下記の手順で評価されます。 + +#### (1) aichallenge_submitの配置 +アップロードしていただいた`aichallenge_submit.tar.gz`は`evaluation/`以下に配置されます。 + +#### (2) docker build +`evaluation/build.sh`が実行され、`evaluation/Dockerfile`で定義されるdockerイメージが作成されます。このイメージの作成手順は下記の通りです。 + +1. 提出いただいた`aichallenge_submit.tar.gz`を`/aichallenge/aichallenge_ws/src/aichallenge_submit`へ展開 +2. `rosdep install`と`colcon build`の実行 + +### (3) シミュレーション実行 +オンライン評価環境でsimulatorが立ち上がり、シミュレーションが開始されます。 + +コンテナ内では`evaluation/main.bash`の実行によって、以下が行われます。 + +1. ROS2ノード群の起動 +2. シナリオの開始 + +`evaluation/run.sh`で実行した場合、`evaluation/output`以下に結果(score.json)が保存されます。 + +## オンライン評価環境にソースコードを提出する際の手順 +### (1) ソースコードを圧縮する +`aichallenge_submit`内のソースコードを圧縮します。 + +```sh +cd evaluation +sh create_submit_tar.sh +``` + +`evaluation/aichallenge_submit.tar.gz`に圧縮済みのファイルが生成されていることを確認してください。 + +### (2) `evaluation/` でdocker内での自動実行ができることを確認する +オンライン評価環境にアップロードする前に、ローカル環境を使いオンライン環境と同様のDockerコンテナ内でビルド・実行ができることを以下の手順で確認してください。 + +まず、以下のファイルが`evaluation/`以下に配置されていることを確認してください。 +- `aichallenge_submit.tar.gz` +- + +次に、作成いただいた`aichallenge_submit`を含むdockerイメージをビルドしてください。 +```sh +sh build.sh +``` + +ビルドが完了したら、`run.sh`によってdockerコンテナを立ち上げ採点のフローを実行してください。 +```sh +sh run.sh +``` + +最後に、`evaluation/output/score.json`に出力されるスコアを確認してください。 + +### (3) オンライン評価環境webページよりソースをアップロードする + +[webページ](https://aichallenge.synesthesias.jp/)にログイン後画面の指示に従って(1)で作成した`aichallenge_submit.tar.gz`をアップロードしてください。 + +アップロードが終了すると、ソースのビルド・シミュレーションの実行が順番に行われます。 + +- 正常に終了した場合は`Scoring complete`と表示され、配布シナリオ・評価用シナリオそれぞれのタイムが表示されます。最後にアップロードした評価シナリオのタイムが、ランキングにて最終タイムとして使われます。 +- 正常にシナリオ実行が終了しても、launchに失敗した等でスコアが出力されていない場合は`No result`、チェックポイントを全て通過していない場合は`Checkpoint not passed`と表示され、いずれの場合も最終的なタイムとしては使われません。 +- ビルドに失敗した場合は`Build error`が表示されます。(1),(2)の手順に従ってDocker imageのビルドができることを再度ご確認ください。 +- シミュレーターの実行に失敗した場合は`Simulator error`と表示されます。この場合サーバーサイドで内部エラーが生じている可能性があるため再度アップロードお願いします。繰り返し表示されてしまう場合はお問合せください。 +- 採点プロセスは一度の提出で5回行われ、結果はその平均によって決定されます。 + +なお、採点実行中は新たなソースのアップロードはできません。またアップロードできるのは1日3回までで、日本時間0時にリセットされます。 + ## その他 ### 更新等の通知に関して githubの更新などがある場合は、以下のURLのissueに新たにコメントします。 @@ -232,5 +312,3 @@ https://github.com/AutomotiveAIChallenge/aichallenge2022-sim/issues/1 各issueでの質問については、基本的に2営業日以内に回答いたします。ただし、検討に時間を要する質問や質問数が多い場合等については、2営業日以上いただく可能性があることはご理解ください。 オンラインシミュレータにログインできないなど、オンラインシミュレータのアカウントに関するお問い合わせはinfo-ai@jsae.or.jp宛にお願いいたします。 - - diff --git a/README_en.md b/README_en.md index 57b6147..4781e9b 100644 --- a/README_en.md +++ b/README_en.md @@ -118,16 +118,23 @@ Please install the following: - [Nvidia Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html) - [git lfs](https://packagecloud.io/github/git-lfs/install) - [ROS2](https://docs.ros.org/en/galactic/index.html)(Confirmed Operation:Galactic) + #### **Starting Docker Image and Autoware** + +##### **About Docker image (v3.1) (added on 1/6/2023)** +Docker image is [autoware(fc50327ec926d5c9a04d385581f102a418af0403)](https://github.com/autowarefoundation/autoware/commit/fc50327ec926 d5c9a04d385581f102a418af0403) with the following applied. +- [fix(pointcloud_preprocessor): add missed target dependency #2101](https://github.com/autowarefoundation/autoware.universe/pull/2101/files) with the following fixes applied. +- Remove tier4_*_launch package + - Moved to aichallenge_submit, if you want to change autoware's launch, config file, please edit here. + 1. Pull the Docker image using docker pull. ``` -docker pull ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:latest +docker pull ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:3.1 ``` ※If the above method takes a long time or times out, please use the following command. -Please use the following command, as we have placed a tar file of the images at [here](https://drive.google.com/drive/u/2/folders/1VZAcGzcFpOBJlmmybcGau7BaHzZW5Chc). +Please use the following command, as we have placed a tar file of the images at [here](https://drive.google.com/file/d/145HyoeXye_bbdT6tOVVCvbSM1MTm2CKI/view?usp=share_link). ```` -gzip -d aichallenge2022_sim_autoware_v2.0.tar.gz -docker load < aichallenge2022_sim_autoware_v2.0.tar +docker load < aichallenge2022_sim_autoware_v3.1.tar.gz ```` 2. Get the data for the competition. @@ -139,7 +146,7 @@ git lfs clone https://github.com/AutomotiveAIChallenge/aichallenge2022-sim 3. Start rocker. ``` cd ./aichallenge2022-sim -rocker --nvidia --x11 --user --net host --privileged --volume autoware:/aichallenge -- ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:latest +rocker --nvidia --x11 --user --net host --privileged --volume autoware:/aichallenge -- ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:3.1 ``` ### **Sample code (ROS2 package)** @@ -159,6 +166,11 @@ We provide the following ROS2 package in `autoware/aichallenge_ws/src` as a samp - Since `aichallenge_submit_launch.launch.xml` is called from the original launch file `aichallenge.launch.xml`, so please modify this launch file so that the ROS2 node in which you are implemented will be launched. - sample_code_cpp - This is a sample automatic run implementation. + - obstacle_stop_planner_custom + - Fixes a problem with false detection of obstacles from obstacle_stop_planner in autoware.universe. + - tier4_*_launch + - This is a copy of autoware's launch file, partially edited. autoware's tier4_*_launch has been deleted, so be sure to leave this one in aichallenge_submit. + - It has been modified to call obstacle_stop_planner_custom instead of obstacle_stop_planner. ### **sample code build** ```` @@ -217,6 +229,75 @@ ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehic ## Time Measurement Please refer to [RULE_en.md](/RULE_en.md) for the time acquisition method. +## About the online evaluation environment +### Outline of the execution flow in the online environment during evaluation +To calculate the score, only the package `aichallenge_submit` is submitted from the web page of the online evaluation environment for automatic scoring. +After submission, the online evaluation environment uses the scripts under `evaluation/` to perform the following steps. + +#### (1) Deployment of `aichallenge_submit +The uploaded `aichallenge_submit.tar.gz` will be placed under `evaluation/`. + +#### (2) docker build +The `evaluation/build.sh` will be executed to create the docker image defined in `evaluation/Dockerfile`. The procedure for creating this image is as follows + +1. extract the submitted `aichallenge_submit.tar.gz` to `/aichallenge/aichallenge_ws/src/aichallenge_submit +2. run `rosdep install` and `colcon build + +### (3) Simulation run +simulator will be launched in the online evaluation environment and simulation will be started. + +In the container, by executing `evaluation/main.bash`, the following will be performed: + +1. start ROS2 nodes +2. start of scenario + +If executed in `evaluation/run.sh`, the results (score.json) will be saved under `evaluation/output`. + +## Procedure for submitting source code to the online evaluation environment +### (1) Compress your source code. +Compress the source code in `aichallenge_submit`. + +```sh +cd evaluation +sh create_submit_tar.sh +``` + +Make sure that a compressed file is generated in `evaluation/aichallenge_submit.tar.gz`. + +### (2) Confirm that the file can be automatically executed in docker at `evaluation/`. +Before uploading to the online evaluation environment, please confirm that you can build and execute in a Docker container similar to the online environment using your local environment by following the steps below. + +First, make sure the following files are located under `evaluation/`. +- `aichallenge_submit.tar.gz` +-aichallenge_submit.tar.gz`. + +Next, build the docker image containing the `aichallenge_submit` you created. +```sh +sh build.sh +``` + +Once the build is complete, launch the docker container with `run.sh` and execute the scoring flow. +```sh +sh run.sh +``` + +Finally, check the scores output to `evaluation/output/score.json`. + +### (3) Upload the source code from the online evaluation environment web page + +Upload the `aichallenge_submit.tar.gz` created in (1) according to the instructions on the screen after logging in to the [web page](https://aichallenge.synesthesias.jp/). + +After the upload is finished, the source build and simulation will be executed in order. + +- If it is successfully completed, the message `Scoring complete` will be displayed, and the time for each of the distribution and evaluation scenarios will be shown. The time of the last uploaded evaluation scenario will be used as the final time in the ranking. +- Even if the scenario execution finishes successfully, `No result` will be displayed if no score is output due to launch failure, etc., or `Checkpoint not passed` if all checkpoints have not been passed, and in any case, the time will not be used as the final time. +- If the build fails, `Build error` is displayed. Please reconfirm that you can build the Docker image by following the steps (1) and (2). +- If the simulator fails to run, you will see `Simulator error`. In this case, there may be an internal error on the server side, so please upload the image again. If the error message is displayed repeatedly, please contact us. +- The grading process will be performed 5 times per submission, and the result will be determined by the average of the 5 times. + +Please note that you will not be able to upload new sources while the grading process is in progress. Uploading is limited to 3 times a day and will be reset at midnight Japan time. + + ## Others ### Notification of updates When there are updates on GitHub, we will make a new comment on the issue at the following URL. Please SUBSCRIBE to this issue to be notified of updates (please turn on notifications)https://github.com/AutomotiveAIChallenge/aichallenge2022-sim/issues/1. diff --git a/autoware/aichallenge_ws/src/aichallenge_eval/src/score.cpp b/autoware/aichallenge_ws/src/aichallenge_eval/src/score.cpp index da47624..e3c1a5e 100644 --- a/autoware/aichallenge_ws/src/aichallenge_eval/src/score.cpp +++ b/autoware/aichallenge_ws/src/aichallenge_eval/src/score.cpp @@ -9,11 +9,13 @@ #include "std_msgs/msg/float64.hpp" #include +#include +#include class Score : public rclcpp::Node { public: - Score() : Node("score_node") + Score() : system_clock(RCL_ROS_TIME), Node("score_node") { // publisher score_publisher = @@ -33,6 +35,9 @@ class Score : public rclcpp::Node stop_point_collision_subscriber = this->create_subscription( "/score/not_stop_collision", 1, std::bind(&Score::stopPointCollisionCallback, this, std::placeholders::_1)); + timer = this->create_wall_timer( + std::chrono::duration(1.0), + std::bind(&Score::timerCallback, this)); } private: @@ -41,6 +46,7 @@ class Score : public rclcpp::Node { std::cout << "Start:" << msg.data << std::endl; start_time = msg.data; + has_start_time_set = true; } // Subscribe The Check Point Time void checkTimeCallback(const std_msgs::msg::Int64 msg) @@ -65,6 +71,7 @@ class Score : public rclcpp::Node result_msg.has_exceeded_speed_limit = false; result_msg.check_point_count = check_count; score_publisher->publish(result_msg); + writeResultJson(result_msg); has_published_result = true; } @@ -85,6 +92,7 @@ class Score : public rclcpp::Node result_msg.has_exceeded_speed_limit = true; result_msg.check_point_count = check_count; score_publisher->publish(result_msg); + writeResultJson(result_msg); has_published_result = true; } @@ -105,6 +113,7 @@ class Score : public rclcpp::Node result_msg.has_collided = true; result_msg.check_point_count = check_count; score_publisher->publish(result_msg); + writeResultJson(result_msg); has_published_result = true; } @@ -125,10 +134,53 @@ class Score : public rclcpp::Node result_msg.has_exceeded_speed_limit = false; result_msg.check_point_count = check_count; score_publisher->publish(result_msg); + writeResultJson(result_msg); has_published_result = true; } + void timerCallback() { + if (!has_start_time_set) + return; + + auto sec = system_clock.now().seconds(); + if (!is_start_sec_initialized) { + start_sec = sec; + is_start_sec_initialized = true; + } + + if (sec - start_sec < 10 * 60) + return; + + if (has_published_result) + return; + + std::cout << "Timeout" << std::endl; + + result_msg.score = 0; + result_msg.has_finished = false; + result_msg.has_park_failed = false; + result_msg.has_collided = false; + result_msg.has_exceeded_speed_limit = false; + result_msg.check_point_count = check_count; + score_publisher->publish(result_msg); + writeResultJson(result_msg); + + has_published_result = true; + } + + void writeResultJson(const aichallenge_score_msgs::msg::Score& score_msg) { + std::ofstream ofs("score.json"); + ofs << "{" << std::endl; + ofs << " \"score\": " << score_msg.score << "," << std::endl; + ofs << " \"hasFinished\": " << score_msg.has_finished << "," << std::endl; + ofs << " \"hasCollided\": " << score_msg.has_collided << "," << std::endl; + ofs << " \"hasExceededSpeedLimit\": " << score_msg.has_exceeded_speed_limit << "," << std::endl; + ofs << " \"checkPointCount\": " << score_msg.check_point_count << "," << std::endl; + ofs << "}" << std::endl; + ofs.close(); + } + // publisher rclcpp::Publisher::SharedPtr score_publisher; @@ -140,11 +192,18 @@ class Score : public rclcpp::Node rclcpp::Subscription::SharedPtr vehicle_collision_subscriber; rclcpp::Subscription::SharedPtr stop_point_collision_subscriber; + rclcpp::TimerBase::SharedPtr timer; + + rclcpp::Clock system_clock; + double start_sec = 0; + bool is_start_sec_initialized = false; + // msg aichallenge_score_msgs::msg::Score result_msg = aichallenge_score_msgs::msg::Score(); // other - int start_time = -99999999; + int start_time = 0; + bool has_start_time_set = false; int end_time = 0; int score_time = 0; int check_count = 0; diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/CMakeLists.txt new file mode 100644 index 0000000..1e9c0bc --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.14) +project(obstacle_stop_planner_custom) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(PCL REQUIRED) + +ament_auto_add_library(obstacle_stop_planner SHARED + src/debug_marker.cpp + src/node.cpp + src/planner_utils.cpp + src/adaptive_cruise_control.cpp +) + +target_include_directories(obstacle_stop_planner + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +target_link_libraries(obstacle_stop_planner + ${OpenCV_LIBRARIES} + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(obstacle_stop_planner + PLUGIN "motion_planning::ObstacleStopPlannerNode" + EXECUTABLE obstacle_stop_planner_node +) + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/README.md new file mode 100644 index 0000000..a76b8a9 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/README.md @@ -0,0 +1,360 @@ +# Obstacle Stop Planner + +## Overview + +`obstacle_stop_planner` has following modules + +- Obstacle Stop Planner + - inserts a stop point in trajectory when there is a static point cloud on the trajectory. +- Slow Down Planner + - inserts a deceleration section in trajectory when there is a point cloud near the trajectory. +- Adaptive Cruise Controller (ACC) + - embeds target velocity in trajectory when there is a dynamic point cloud on the trajectory. + +### Input topics + +| Name | Type | Description | +| --------------------------- | ----------------------------------------------- | ------------------- | +| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | +| `~/input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map | +| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | +| `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | + +### Output topics + +| Name | Type | Description | +| ---------------------- | --------------------------------------- | -------------------------------------- | +| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | +| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | + +### Common Parameter + +| Parameter | Type | Description | +| ------------------- | ------ | -------------------------------------------------------------------------------------- | +| `enable_slow_down` | bool | enable slow down planner [-] | +| `max_velocity` | double | max velocity [m/s] | +| `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | + +## Obstacle Stop Planner + +### Role + +This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `baselink`(center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points for reducing computational costs.) + +
+ ![example](./docs/collision_parameters.svg){width=1000} +
parameters for obstacle stop planner
+
+ +
+ ![example](./docs/stop_target.svg){width=1000} +
target for obstacle stop planner
+
+ +If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.) + +
+ ![example](./docs/min_longitudinal_margin.svg){width=1000} +
minimum longitudinal margin
+
+ +The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. + +### Restart prevention + +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). + +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module, the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. + +
+ ![example](./docs/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](./docs/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](./docs/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
+ +### Parameters + +#### Stop position + +| Parameter | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------- | +| `max_longitudinal_margin` | double | margin between obstacle and the ego's front [m] | +| `min_longitudinal_margin` | double | if any obstacle exists within `max_longitudinal_margin`, this module set margin as the value of _stop margin_ to `min_longitudinal_margin` [m] | +| `hold_stop_margin_distance` | double | parameter for restart prevention (See above section) [m] | + +#### Obstacle detection area + +| Parameter | Type | Description | +| ----------------- | ------ | ----------------------------------------------------------------------------------- | +| `lateral_margin` | double | lateral margin from the vehicle footprint for collision obstacle detection area [m] | +| `step_length` | double | step length for pointcloud search range [m] | +| `extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] | + +### Flowchart + +```plantuml +@startuml +title insertStopPoint +start + + +:callback trajectory; + +:trim trajectory from self-pose; + +:decimate trajectory; + +:generate detection area; + +if ( search obstacle pointcloud in detection area?) then (yes(find)) +else (no(not find)) + stop +endif + +if ( insert target velocity by adaptive cruise module?) then (yes) + stop +else (no) +endif + +:insert stop point; + + +stop +@enduml +``` + +## Slow Down Planner + +### Role + +This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The velocity is calculated the following equation. + +$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} )$ + +- $v_{target}$ : slow down target velocity [m/s] +- $v_{min}$ : `min_slow_down_velocity` [m/s] +- $v_{max}$ : `max_slow_down_velocity` [m/s] +- $l_{ld}$ : lateral deviation between the obstacle and the ego footprint [m] +- $l_{margin}$ : `lateral_margin` [m] +- $l_{vw}$ : width of the ego footprint [m] + +The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow down section. + +
+ ![example](./docs/slow_down_parameters.svg){width=1000} +
parameters for slow down planner
+
+ +
+ ![example](./docs/slow_down_target.svg){width=1000} +
target for slow down planner
+
+ +### Parameters + +#### Slow down section + +| Parameter | Type | Description | +| ------------------------------ | ------ | ----------------------------------------------- | +| `longitudinal_forward_margin` | double | margin between obstacle and the ego's front [m] | +| `longitudinal_backward_margin` | double | margin between obstacle and the ego's rear [m] | + +#### Obstacle detection area + +| Parameter | Type | Description | +| ---------------- | ------ | ----------------------------------------------------------------------------------- | +| `lateral_margin` | double | lateral margin from the vehicle footprint for slow down obstacle detection area [m] | + +#### Slow down target velocity + +| Parameter | Type | Description | +| ------------------------ | ------ | ---------------------------- | +| `max_slow_down_velocity` | double | max slow down velocity [m/s] | +| `min_slow_down_velocity` | double | min slow down velocity [m/s] | + +### Flowchart + +```plantuml +@startuml +title insertDecelerationPoint +start + + +:callback trajectory; + +:trim trajectory from self-pose; + +:decimate trajectory; + +:generate detection area; + +if ( search obstacle pointcloud in detection area?) then (yes(find)) +else (no(not find)) + stop +endif + +:insert deceleration point; + + +stop +@enduml +``` + +## Adaptive Cruise Controller + +### Role + +`Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of the front car), and the distance to the point cloud (= distance to the front car). + +| Parameter | Type | Description | +| ---------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------- | +| `adaptive_cruise_control.use_object_to_estimate_vel` | bool | use dynamic objects for estimating object velocity or not | +| `adaptive_cruise_control.use_pcl_to_estimate_vel` | bool | use raw pointclouds for estimating object velocity or not | +| `adaptive_cruise_control.consider_obj_velocity` | bool | consider forward vehicle velocity to calculate target velocity in adaptive cruise or not | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_start_acc` | double | start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_stop_acc` | double | stop acc when the velocity of the forward obstacle falls below this value [m/s] | +| `adaptive_cruise_control.emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.emergency_stop_idling_time` | double | supposed idling time to start emergency stop [s] | +| `adaptive_cruise_control.min_dist_stop` | double | minimum distance of emergency stop [m] | +| `adaptive_cruise_control.obstacle_emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.max_standard_acceleration` | double | supposed maximum acceleration in active cruise control [m/ss] | +| `adaptive_cruise_control.min_standard_acceleration` | double | supposed minimum acceleration (deceleration) in active cruise control [m/ss] | +| `adaptive_cruise_control.standard_idling_time` | double | supposed idling time to react object in active cruise control [s] | +| `adaptive_cruise_control.min_dist_standard` | double | minimum distance in active cruise control [m] | +| `adaptive_cruise_control.obstacle_min_standard_acceleration` | double | supposed minimum acceleration of forward obstacle [m/ss] | +| `adaptive_cruise_control.margin_rate_to_change_vel` | double | rate of margin distance to insert target velocity [-] | +| `adaptive_cruise_control.use_time_compensation_to_calc_distance` | bool | use time-compensation to calculate distance to forward vehicle | +| `adaptive_cruise_control.p_coefficient_positive` | double | coefficient P in PID control (used when target dist -current_dist >=0) [-] | +| `adaptive_cruise_control.p_coefficient_negative` | double | coefficient P in PID control (used when target dist -current_dist <0) [-] | +| `adaptive_cruise_control.d_coefficient_positive` | double | coefficient D in PID control (used when delta_dist >=0) [-] | +| `adaptive_cruise_control.d_coefficient_negative` | double | coefficient D in PID control (used when delta_dist <0) [-] | +| `adaptive_cruise_control.object_polygon_length_margin` | double | The distance to extend the polygon length the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.object_polygon_width_margin` | double | The distance to extend the polygon width the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.valid_estimated_vel_diff_time` | double | Maximum time difference treated as continuous points in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_vel_que_time` | double | Time width of information used for speed estimation in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_estimated_vel_max` | double | Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.valid_estimated_vel_min` | double | Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.thresh_vel_to_stop` | double | Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] | +| `adaptive_cruise_control.lowpass_gain_of_upper_velocity` | double | Lowpass-gain of target velocity | +| `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed | +| `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value | + +### Flowchart + +```plantuml +@startuml +title insertTargetVelocity() +start + +:get target vehicle point (*1) in detection area ; + +partition Estimate-Target-Velocity { + +if (Is there a DynamicObject on the target vehicle point?) then (yes) +:use the DynamicObject velocity\nas a target vehicle point velocity (*2); +else (no) + + if (The target vehicle point is found in the previous step?) then (yes) + else (no (estimation failed)) + stop + endif + + :estimate the target vehicle point velocity \nby the travel distance from the previous step; + + if (The estimated velocity is valid?) then (yes) + else (no (estimation failed)) + stop + endif + + :use the estimated velocity\nas a target vehicle point velocity (*2); + +endif +} + +if (Is the target vehicle point velocity fast enough?) then (yes) +else (no) + stop +endif + +if (calculate distance to the pointcloud from self-position?) then (yes) +else (no) + stop +endif + +:calculate target velocity to be inserted in the trajectory; + +if (the target velocity is fast enough?) then (yes) +else (no) + stop +endif + +:insert the target velocity; + +stop +@enduml +``` + +(\*1) The target vehicle point is calculated as a closest obstacle PointCloud from ego along the trajectory. + +(\*2) The sources of velocity estimation can be changed by the following ROS parameters. + +- `adaptive_cruise_control.use_object_to_estimate_vel` +- `adaptive_cruise_control.use_pcl_to_estimate_vel` + +This module works only when the target point is found in the detection area of the `Obstacle stop planner` module. + +The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the perception failure. +If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the dynamic object is used as the target point velocity. +Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; that is `(current_position - previous_position) / dt`. Note that this travel distance based estimation fails when the target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the estimation, the median of the calculation result for several steps is used. + +If the calculated velocity is within the threshold range, it is used as the target point velocity. + +Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity to insert is calculated. + +The emergency distance $d\_{emergency}$ is calculated as follows. + +$d_{emergency} = d_{margin_{emergency}} + t_{idling_{emergency}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{emergency}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{emergency}}})$ + +- $d_{margin_{emergency}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{emergency}}$ depends on the parameter `min_dist_stop` +- $t_{idling_{emergency}}$ is a supposed idling time. The value of $t_{idling_{emergency}}$ depends on the parameter `emergency_stop_idling_time` +- $v_{ego}$ is a current velocity of own vehicle +- $a_{ego_{_{emergency}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{emergency}}}$ depends on the parameter `emergency_stop_acceleration` +- $v_{obj}$ is a current velocity of obstacle pointcloud. +- $a_{obj_{_{emergency}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{emergency}}}$ depends on the parameter `obstacle_emergency_stop_acceleration` +- \*Above $X_{_{emergency}}$ parameters are used only in emergency situation. + +The target velocity is determined to keep the distance to the obstacle pointcloud from own vehicle at the standard distance $d\_{standard}$ calculated as following. Therefore, if the distance +to the obstacle pointcloud is longer than standard distance, The target velocity becomes higher than the current velocity, and vice versa. For keeping the distance, a PID controller is used. + +$d_{standard} = d_{margin_{standard}} + t_{idling_{standard}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{standard}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{standard}}})$ + +- $d_{margin_{standard}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{standard}}$ depends on the parameter `min_dist_stop` +- $t_{idling_{standard}}$ is a supposed idling time. The value of $t_{idling_{standard}}$ depends on the parameter `standard_stop_idling_time` +- $v_{ego}$ is a current velocity of own vehicle +- $a_{ego_{_{standard}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{standard}}}$ depends on the parameter `min_standard_acceleration` +- $v_{obj}$ is a current velocity of obstacle pointcloud. +- $a_{obj_{_{standard}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{standard}}}$ depends on the parameter `obstacle_min_standard_acceleration` +- \*Above $X_{_{standard}}$ parameters are used only in non-emergency situation. + +![adaptive_cruise](./docs/adaptive_cruise.drawio.svg) + +If the target velocity exceeds the value of `thresh_vel_to_stop`, the target velocity is embedded in the trajectory. + +## Known Limits + +- It is strongly depends on velocity planning module whether or not it moves according to the target speed embedded by `Adaptive Cruise Controller` module. If the velocity planning module is updated, please take care of the vehicle's behavior as much as possible and always be ready for overriding. + +- The velocity estimation algorithm in `Adaptive Cruise Controller` is depend on object tracking module. Please note that if the object-tracking fails or the tracking result is incorrect, it the possibility that the vehicle behaves dangerously. + +- It does not work for backward driving, but publishes the path of the input as it is. Please use [obstacle_cruise_planner](../obstacle_cruise_planner/README.md) if you want to stop against an obstacle when backward driving. diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/adaptive_cruise_control.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/adaptive_cruise_control.param.yaml new file mode 100644 index 0000000..691be53 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/adaptive_cruise_control.param.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + adaptive_cruise_control: + # Adaptive Cruise Control (ACC) config + use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not + use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not + consider_obj_velocity: true # consider forward vehicle velocity to ACC or not + + # general parameter for ACC + obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] + obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] + emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] + min_dist_stop: 4.0 # minimum distance of emergency stop [m] + obstacle_emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] + min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control + standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] + min_dist_standard: 4.0 # minimum distance in active cruise control [m] + obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] + margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] + use_time_compensation_to_calc_distance: true # use time-compensation to calculate distance to forward vehicle + + # pid parameter for ACC + p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] + p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] + d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] + d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] + + # parameter for object velocity estimation + object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] + object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] + valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] + valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] + valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] + valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] + thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] + lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity + use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) + rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/common.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/common.param.yaml new file mode 100644 index 0000000..a23570a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/common.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # constraints param for normal driving + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/obstacle_stop_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/obstacle_stop_planner.param.yaml new file mode 100644 index 0000000..48dba07 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/obstacle_stop_planner.param.yaml @@ -0,0 +1,50 @@ +/**: + ros__parameters: + hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + max_velocity: 20.0 # max velocity [m/s] + enable_slow_down: False # whether to use slow down planner [-] + enable_z_axis_obstacle_filtering: True + z_axis_filtering_buffer: 0.0 + + stop_planner: + # params for stop position + stop_position: + max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_longitudinal_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] + hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] + + # params for detection area + detection_area: + lateral_margin: 0.0 # margin of vehicle footprint [m] + step_length: 1.0 # step length for pointcloud search range [m] + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] + + + slow_down_planner: + # params for slow down section + slow_down_section: + longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] + longitudinal_backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m] + longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + + # params for detection area + detection_area: + lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + + # params for velocity + target_velocity: + max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s] + min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s] + slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s] + + # params for deceleration constraints (use this param if consider_constraints is True) + constraints: + jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss] + jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] + jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] + + # others + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] + velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] + acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/plot_juggler_adaptive_cruise.xml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/plot_juggler_adaptive_cruise.xml new file mode 100644 index 0000000..5770820 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/plot_juggler_adaptive_cruise.xml @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/plot_juggler_slow_down.xml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/plot_juggler_slow_down.xml new file mode 100644 index 0000000..cbb840b --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/plot_juggler_slow_down.xml @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/rqt_multiplot_adaptive_cruise.xml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/rqt_multiplot_adaptive_cruise.xml new file mode 100644 index 0000000..b3c3b49 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/config/rqt_multiplot_adaptive_cruise.xml @@ -0,0 +1,669 @@ + + + + #babdb6 + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Estimated Object Velocity(from pcl) + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Estimated Object Velocity(from dynamic object) + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Estimated Object Velocity(final) + + + + true + + 30 + Estimated Object Velocity + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Current Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/8 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Raw Upper Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/9 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Upper Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Target Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Closest Velocity + + + + true + + 30 + Ego Velocity + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Distance to Forward Object + + + + true + + 30 + Distance + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/25 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Current Accel + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Target Accel + + + + true + + 30 + Accel + + + + false +
+
diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/adaptive_cruise.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/adaptive_cruise.drawio.svg new file mode 100644 index 0000000..a13a31e --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/adaptive_cruise.drawio.svg @@ -0,0 +1,181 @@ + + + + + + + + + + + + +
+
+
+ Ego vehicle +
+
+
+
+ + Ego vehicle + +
+
+ + + + +
+
+
+ (Front vehicle) +
+
+
+
+ + (Front vehicle) + +
+
+ + + + +
+
+
+ Obstacle Pointcloud +
+
+
+
+ + Obstacle Pointcloud + +
+
+ + + + + + + + + + + + + + + + + + +
+
+
+ distance to obstacle pointcloud +
+
+
+
+ + distance to obstacle pointcloud + +
+
+ + + + + + +
+
+
+ Emergency Distance +
+
+
+
+ + Emergency Distance + +
+
+ + + + +
+
+
+ Standard Distance +
+
+
+
+ + Standard Distance + +
+
+ + + + +
+
+
+ + When the distance to obstacle pointcloud is shorter than standard distance, +
+ target velocity becomes higher than current velocity and vice versa +
+
+
+
+
+ + When the distance to obstacle pointcloud is shorter than s... + +
+
+ + + + +
+
+
+ + When the distance to obstacle pointcloud is shorter than emergency distance, +
+ Adaptive Cruise Controller modules does not works and +
+ a stop point is inserted by Obstacle Stop Planner module. +
+
+
+
+
+
+ + When the distance to obstacle pointcloud is shorter than eme... + +
+
+
+ + + + + Viewer does not support full SVG 1.1 + + + +
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/collision_parameters.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/collision_parameters.svg new file mode 100644 index 0000000..a1f3c34 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/collision_parameters.svg @@ -0,0 +1,3 @@ + + +
Obstacle
Obsta...
velocity
veloc...
baselink_to_front [m]
baselink_to_front [m]
max_longitudinal_margin [m]
max_longitudinal_margin [m]
reference velocity
reference velocity
output velocity
output velocity
travel
distance
trave...
trajectory
trajectory
Ego
Ego
lateral_margin [m]
lateral_margin [m]
vehicle_width / 2 + lateral_margin [m]
vehicle_width / 2 + lateral_margin...
detection area for collision obstacles
detection area for collision obstacl...
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/keep_stopping.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/keep_stopping.svg new file mode 100644 index 0000000..50f344c --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/keep_stopping.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
keep stopping at current position
keep stopping at curr...
target stop position
target stop position
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/min_longitudinal_margin.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/min_longitudinal_margin.svg new file mode 100644 index 0000000..fb42e7f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/min_longitudinal_margin.svg @@ -0,0 +1,3 @@ + + +
Obstacle
Obsta...
velocity
veloc...
baselink_to_front [m]
baselink_to_front [m]
min_longitudinal_margin [m]
min_longitudinal_margin [m]
reference velocity
reference velocity
output velocity
output velocity
travel
distance
trave...
reference trajectory already has stop point
reference trajectory alrea...
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/restart.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/restart.svg new file mode 100644 index 0000000..c21c6d8 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/restart.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
move toward stop line
move toward stop line
target stop position
target stop position
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/restart_prevention.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/restart_prevention.svg new file mode 100644 index 0000000..cc38ad3 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/restart_prevention.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
target stop position
target stop position
hold_stop_margin_distance [m]
hold_stop_margin_distance [m]
front_to_stop_line [m]
front_to_stop_line [...
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/slow_down_parameters.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/slow_down_parameters.svg new file mode 100644 index 0000000..dcfcd62 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/slow_down_parameters.svg @@ -0,0 +1,3 @@ + + +
trajectory
trajectory
Ego
Ego
lateral_margin [m]
lateral_margin [m]
vehicle_width / 2 + lateral_margin [m]
vehicle_width / 2 + lateral_margin [...
detection area for slow down obstacles
detection area for slow down obstacl...
Obstacle
Obsta...
velocity
veloc...
travel
distance
trave...
longitudinal_forward_margin [m]
longitudinal_forward_margin [m]
reference velocity
reference velocity
output velocity
output velocity
longitudinal_backward_margin [m]
longitudinal_backward_margin [m]
baselink_to_rear [m]
baselink_to_rear [m]
slow_down_velocity [m/s]
slow_down_velocity [m/s]
baselink_to_front [m]
baselink_to_front [m]
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/slow_down_target.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/slow_down_target.svg new file mode 100644 index 0000000..203d703 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/slow_down_target.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
detection area for slow down obstacles
detection area for slow down obstacl...
Target
Target
Not-target
(Target for stop planner)
Not-t...
Not-Target
Not-T...
Target
Target
detection area for collision obstacles
detection area for collision obstacl...
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/stop_target.svg b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/stop_target.svg new file mode 100644 index 0000000..838321b --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/docs/stop_target.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
detection area for collision obstacles
detection area for collision obstacl...
Target
Target
Not-target
Not-t...
Not-Target
Not-T...
Target
Target
Text is not SVG - cannot display
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/adaptive_cruise_control.hpp new file mode 100644 index 0000000..b93faad --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -0,0 +1,225 @@ +// 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 OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ +#define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace motion_planning +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +class AdaptiveCruiseController +{ +public: + AdaptiveCruiseController( + rclcpp::Node * node, const double vehicle_width, const double vehicle_length, + const double baselink2front); + + void insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); + +private: + rclcpp::Publisher::SharedPtr pub_debug_; + + rclcpp::Node * node_; + /* + * Parameter + */ + double vehicle_width_; + double vehicle_length_; + double baselink2front_; + + rclcpp::Time prev_collision_point_time_; + pcl::PointXYZ prev_collision_point_; + double prev_target_vehicle_time_ = 0.0; + double prev_target_vehicle_dist_ = 0.0; + double prev_target_velocity_ = 0.0; + bool prev_collision_point_valid_ = false; + bool prev_obstacle_velocity_judge_to_start_acc_ = false; + std::vector est_vel_que_; + double prev_upper_velocity_ = 0.0; + + struct Param + { + //!< @brief use tracking objects for estimating object velocity or not + bool use_object_to_est_vel; + + //!< @brief use pcl for estimating object velocity or not + bool use_pcl_to_est_vel; + + //!< @brief consider forward vehicle velocity to self upper velocity or not + bool consider_obj_velocity; + + //!< @brief The distance to extend the polygon length the object in pointcloud-object matching + double object_polygon_length_margin; + + //!< @brief The distance to extend the polygon width the object in pointcloud-object matching + double object_polygon_width_margin; + + //!< @brief Maximum time difference treated as continuous points in speed estimation using a + // point cloud + double valid_est_vel_diff_time; + + //!< @brief Time width of information used for speed estimation in speed estimation using a + // point cloud + double valid_vel_que_time; + + //!< @brief Maximum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_max; + + //!< @brief Minimum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_min; + + //!< @brief Embed a stop line if the maximum speed calculated by ACC is lower than this speed + double thresh_vel_to_stop; + + /* parameter for acc */ + //!< @brief start acc when the velocity of the forward obstacle exceeds this value + double obstacle_velocity_thresh_to_start_acc; + + //!< @brief stop acc when the velocity of the forward obstacle falls below this value + double obstacle_velocity_thresh_to_stop_acc; + + //!< @brief supposed minimum acceleration in emergency stop + double emergency_stop_acceleration; + + //!< @brief supposed minimum acceleration of forward vehicle in emergency stop + double obstacle_emergency_stop_acceleration; + + //!< @brief supposed idling time to start emergency stop + double emergency_stop_idling_time; + + //!< @brief minimum distance of emergency stop + double min_dist_stop; + + //!< @brief supposed maximum acceleration in active cruise control + double max_standard_acceleration; + + //!< @brief supposed minimum acceleration(deceleration) in active cruise control + double min_standard_acceleration; + + //!< @brief supposed idling time to react object in active cruise control + double standard_idling_time; + + //!< @brief minimum distance in active cruise control + double min_dist_standard; + + //!< @brief supposed minimum acceleration of forward obstacle + double obstacle_min_standard_acceleration; + + //!< @brief margin to insert upper velocity + double margin_rate_to_change_vel; + + //!< @brief use time-compensation to calculate distance to forward vehicle + bool use_time_compensation_to_dist; + + //!< @brief gain of lowpass filter of upper velocity + double lowpass_gain_; + + //!< @brief when failed to estimate velocity, use rough velocity estimation or not + bool use_rough_est_vel; + + //!< @brief in rough velocity estimation, front car velocity is + //!< estimated as self current velocity * this value + double rough_velocity_rate; + + /* parameter for pid used in acc */ + //!< @brief coefficient P in PID control (used when target dist -current_dist >=0) + double p_coeff_pos; + + //!< @brief coefficient P in PID control (used when target dist -current_dist <0) + double p_coeff_neg; + + //!< @brief coefficient D in PID control (used when delta_dist >=0) + double d_coeff_pos; + + //!< @brief coefficient D in PID control (used when delta_dist <0) + double d_coeff_neg; + + static constexpr double d_coeff_valid_time = 1.0; + static constexpr double d_coeff_valid_diff_vel = 20.0; + static constexpr double d_max_vel_norm = 3.0; + }; + Param param_; + + double getMedianVel(const std::vector vel_que); + double lowpass_filter(const double current_value, const double prev_value, const double gain); + void calcDistanceToNearestPointOnPath( + const TrajectoryPoints & trajectory, const int nearest_point_idx, + const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * distance, + const std_msgs::msg::Header & trajectory_header); + double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx); + bool estimatePointVelocityFromObject( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity); + bool estimatePointVelocityFromPcl( + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * velocity); + double estimateRoughPointVelocity(double current_vel); + bool isObstacleVelocityHigh(const double obj_vel); + double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); + double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel); + double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel); + double calcTargetVelocity_P(const double target_dist, const double current_dist); + double calcTargetVelocity_I(const double target_dist, const double current_dist); + double calcTargetVelocity_D(const double target_dist, const double current_dist); + double calcTargetVelocityByPID( + const double current_vel, const double current_dist, const double obj_vel); + + void insertMaxVelocityToPath( + const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, + const double dist_to_collision_point, TrajectoryPoints * output_trajectory); + void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); + + /* Debug */ + mutable tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values_; + enum DBGVAL { + ESTIMATED_VEL_PCL = 0, + ESTIMATED_VEL_OBJ = 1, + ESTIMATED_VEL_FINAL = 2, + FORWARD_OBJ_DISTANCE = 3, + CURRENT_VEL = 4, + UPPER_VEL_P = 5, + UPPER_VEL_I = 6, + UPPER_VEL_D = 7, + UPPER_VEL_RAW = 8, + UPPER_VEL = 9 + }; + static constexpr unsigned int num_debug_values_ = 10; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/debug_marker.hpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/debug_marker.hpp new file mode 100644 index 0000000..56b9dfc --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/debug_marker.hpp @@ -0,0 +1,153 @@ +// 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 OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ +#define OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#define EIGEN_MPL2_ONLY +#include +#include +namespace motion_planning +{ + +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using std_msgs::msg::Header; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; + +enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown }; + +enum class PointType : int8_t { Stop = 0, SlowDown }; + +enum class PoseType : int8_t { Stop = 0, TargetStop, SlowDownStart, SlowDownEnd }; + +class DebugValues +{ +public: + enum class TYPE { + CURRENT_VEL = 0, + CURRENT_ACC = 1, + CURRENT_FORWARD_MARGIN = 2, + SLOWDOWN_OBSTACLE_DISTANCE = 3, + COLLISION_OBSTACLE_DISTANCE = 4, + FLAG_FIND_COLLISION_OBSTACLE = 5, + FLAG_FIND_SLOW_DOWN_OBSTACLE = 6, + FLAG_ADAPTIVE_CRUISE = 7, + FLAG_EXTERNAL = 8, + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getValuesIdx(const TYPE type) const { return static_cast(type); } + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> getValues() const { return values_; } + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void setValues(const TYPE type, const double val) { values_.at(static_cast(type)) = val; } + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void setValues(const int type, const double val) { values_.at(type) = val; } + +private: + static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); + std::array(TYPE::SIZE)> values_; +}; + +class ObstacleStopPlannerDebugNode +{ +public: + explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); + ~ObstacleStopPlannerDebugNode() {} + bool pushPolygon( + const std::vector & polygon, const double z, const PolygonType & type); + bool pushPolygon(const std::vector & polygon, const PolygonType & type); + bool pushPolyhedron( + const std::vector & polyhedron, const double z_min, const double z_max, + const PolygonType & type); + bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); + bool pushPose(const Pose & pose, const PoseType & type); + bool pushObstaclePoint(const Point & obstacle_point, const PointType & type); + bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); + MarkerArray makeVirtualWallMarker(); + MarkerArray makeVisualizationMarker(); + StopReasonArray makeStopReasonArray(); + + void setDebugValues(const DebugValues::TYPE type, const double val) + { + debug_values_.setValues(type, val); + } + void publish(); + +private: + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr pub_debug_values_; + rclcpp::Node * node_; + double base_link2front_; + + std::shared_ptr stop_pose_ptr_; + std::shared_ptr target_stop_pose_ptr_; + std::shared_ptr slow_down_start_pose_ptr_; + std::shared_ptr slow_down_end_pose_ptr_; + std::shared_ptr stop_obstacle_point_ptr_; + std::shared_ptr slow_down_obstacle_point_ptr_; + std::vector> vehicle_polygons_; + std::vector> slow_down_range_polygons_; + std::vector> collision_polygons_; + std::vector> slow_down_polygons_; + std::vector> vehicle_polyhedrons_; + std::vector> collision_polyhedrons_; + + DebugValues debug_values_; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/node.hpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/node.hpp new file mode 100644 index 0000000..b61a5c0 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/node.hpp @@ -0,0 +1,209 @@ +// Copyright 2019 Autoware Foundation +// +// 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_STOP_PLANNER__NODE_HPP_ +#define OBSTACLE_STOP_PLANNER__NODE_HPP_ + +#include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include "obstacle_stop_planner/debug_marker.hpp" +#include "obstacle_stop_planner/planner_data.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace motion_planning +{ + +namespace bg = boost::geometry; + +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using std_msgs::msg::Header; + +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_debug_msgs::msg::BoolStamped; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::ExpandStopRange; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using vehicle_info_util::VehicleInfo; + +using TrajectoryPoints = std::vector; +using PointCloud = pcl::PointCloud; + +class ObstacleStopPlannerNode : public rclcpp::Node +{ +public: + explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Subscription::SharedPtr sub_trajectory_; + + rclcpp::Subscription::SharedPtr sub_point_cloud_; + + rclcpp::Subscription::SharedPtr sub_odometry_; + + rclcpp::Subscription::SharedPtr sub_acceleration_; + + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + + rclcpp::Subscription::SharedPtr sub_expand_stop_range_; + + rclcpp::Publisher::SharedPtr pub_trajectory_; + + rclcpp::Publisher::SharedPtr pub_stop_reason_; + + rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; + + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + + rclcpp::Publisher::SharedPtr pub_point_cloud_; + + std::unique_ptr acc_controller_; + std::shared_ptr debug_ptr_; + boost::optional latest_stop_point_{boost::none}; + boost::optional latest_slow_down_section_{boost::none}; + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; + PointCloud2::SharedPtr obstacle_ros_pointcloud_debug_ptr_{nullptr}; + PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; + rclcpp::Time last_detect_time_collision_point_; + rclcpp::Time last_detect_time_slowdown_point_; + + Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; + AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_{nullptr}; + bool is_driving_forward_{true}; + + bool set_velocity_limit_{false}; + + VehicleInfo vehicle_info_; + NodeParam node_param_; + StopParam stop_param_; + SlowDownParam slow_down_param_; + + // mutex for vehicle_info_, stop_param_, current_acc_, obstacle_ros_pointcloud_ptr_ + // NOTE: shared_ptr itself is thread safe so we do not have to care if *ptr is not used + // (current_velocity_ptr_) + std::mutex mutex_; + + void searchObstacle( + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr); + + void insertVelocity( + TrajectoryPoints & trajectory, PlannerData & planner_data, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, + const StopParam & stop_param); + + bool searchPointcloudNearTrajectory( + const TrajectoryPoints & trajectory, const PointCloud2::ConstSharedPtr & input_points_ptr, + PointCloud::Ptr output_points_ptr, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const StopParam & stop_param); + + StopPoint createTargetPoint( + const int idx, const double margin, const TrajectoryPoints & base_trajectory, + const double dist_remain); + + StopPoint searchInsertPoint( + const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain, + const StopParam & stop_param); + + SlowDownSection createSlowDownSection( + const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, + const double dist_remain, const double dist_vehicle_to_obstacle, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel); + + SlowDownSection createSlowDownSectionFromMargin( + const int idx, const TrajectoryPoints & base_trajectory, const double forward_margin, + const double backward_margin, const double velocity); + + void insertSlowDownSection(const SlowDownSection & slow_down_section, TrajectoryPoints & output); + + TrajectoryPoints trimTrajectoryWithIndexFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose, size_t & index); + + void setExternalVelocityLimit(); + + void resetExternalVelocityLimit(const double current_acc, const double current_vel); + + void publishDebugData( + const PlannerData & planner_data, const double current_acc, const double current_vel); + + // Callback + void onTrigger(const Trajectory::ConstSharedPtr input_msg); + + void onOdometry(const Odometry::ConstSharedPtr input_msg); + + void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr input_msg); + + void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + + void onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg); + + void onExpandStopRange(const ExpandStopRange::ConstSharedPtr input_msg); +}; +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__NODE_HPP_ diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/planner_data.hpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/planner_data.hpp new file mode 100644 index 0000000..b7af3ed --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/planner_data.hpp @@ -0,0 +1,258 @@ +// Copyright 2022 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 OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ +#define OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include + +namespace motion_planning +{ + +using diagnostic_msgs::msg::DiagnosticStatus; +using geometry_msgs::msg::Pose; + +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +struct StopPoint +{ + TrajectoryPoint point{}; + + size_t index; +}; + +struct SlowDownSection +{ + TrajectoryPoint start_point{}; + + TrajectoryPoint end_point{}; + + size_t slow_down_start_idx; + + size_t slow_down_end_idx; + + double velocity; +}; + +struct NodeParam +{ + // set True, slow down for obstacle beside the path + bool enable_slow_down; + + // set True, filter obstacles in z axis + bool enable_z_axis_obstacle_filtering; + + // buffer for z axis filtering [m] + double z_axis_filtering_buffer; + + // max velocity [m/s] + double max_velocity; + + // keep slow down or stop state if obstacle vanished [s] + double hunting_threshold; + + // dist threshold for ego's nearest index + double ego_nearest_dist_threshold; + + // yaw threshold for ego's nearest index + double ego_nearest_yaw_threshold; +}; + +struct StopParam +{ + // ============================== + // params for longitudinal margin + // ============================== + + // margin between obstacle and the ego's front [m] + double max_longitudinal_margin; + + // margin between obstacle and the ego's front [m] + // if any other stop point is inserted within max_longitudinal_margin. + double min_longitudinal_margin; + + // ================================== + // params for obstacle detection area + // ================================== + + // lateral margin between the ego's footprint and + // the boundary of the detection area for collision obstacles [m] + // if any obstacles exist within the detection area, this module plans to stop + // before the obstacle. + double lateral_margin; + + // ================================= + // params for trajectory pre-process + // ================================= + + // trajectory extend distance [m] + double extend_distance; + + // step length for pointcloud search range [m] + double step_length; + + // ====== + // others + // ====== + + // search radius for obstacle point cloud [m] + double stop_search_radius; + + // keep stopping if the ego is in this margin [m] + double hold_stop_margin_distance; +}; + +struct SlowDownParam +{ + // ================= + // params for margin + // ================= + + // margin between obstacle and the ego's front [m] + double longitudinal_forward_margin; + + // margin between obstacle and the ego's rear [m] + double longitudinal_backward_margin; + + // OPTIONAL (use this param if consider_constraints is True) + // minimum distance between obstacle and the ego's front when slow down margin is relaxed [m] + double min_longitudinal_forward_margin; + + // OPTIONAL (use this param if consider_constraints is True) + // fineness param for relaxing slow down margin [m] + double longitudinal_margin_span; + + // ================================== + // params for obstacle detection area + // ================================== + + // lateral margin between the ego's footprint and the boundary of the detection area for slow down + // obstacles [m] + double lateral_margin; + + // =================== + // params for velocity + // =================== + + // OPTIONAL (use this param if consider_constraints is False) + // maximum velocity fow slow down section [m/s] + double max_slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is False) + // minimum velocity for slow down section [m/s] + double min_slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is True) + // target slow down velocity [m/s] + double slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is True) + // velocity threshold whether the vehicle complete deceleration [m/s] + double velocity_threshold_decel_complete; + + // OPTIONAL (use this param if consider_constraints is True) + // acceleration threshold whether the vehicle complete deceleration [m/ss] + double acceleration_threshold_decel_complete; + + // =================================== + // params for deceleration constraints + // =================================== + + // OPTIONAL (use this param if consider_constraints is True) + // min jerk limit for mild stop [m/sss] + double normal_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // min deceleration limit for mild stop [m/ss] + double normal_min_acc; + + // OPTIONAL (use this param if consider_constraints is True) + // min jerk limit [m/sss] + double limit_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // min deceleration limit [m/ss] + double limit_min_acc; + + // OPTIONAL (use this param if consider_constraints is True) + // min slow down jerk constraint [m/sss] + double slow_down_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // init jerk used for deceleration planning [m/sss] + double jerk_start; + + // OPTIONAL (use this param if consider_constraints is True) + // fineness param for planning deceleration jerk [m/sss] + double jerk_span; + + // ====== + // others + // ====== + + // set "True", decel point is planned under jerk/dec constraints + bool consider_constraints; + + // search radius for slow down obstacle point cloud [m] + double slow_down_search_radius; +}; + +struct PlannerData +{ + DiagnosticStatus stop_reason_diag{}; + + Pose current_pose{}; + + pcl::PointXYZ nearest_collision_point; + + pcl::PointXYZ nearest_slow_down_point; + + pcl::PointXYZ lateral_nearest_slow_down_point; + + rclcpp::Time nearest_collision_point_time{}; + + double lateral_deviation{0.0}; + + size_t trajectory_trim_index{}; + + size_t decimate_trajectory_collision_index{}; + + size_t decimate_trajectory_slow_down_index{}; + + // key: decimate index, value: original index + std::map decimate_trajectory_index_map{}; + + bool found_collision_points{false}; + + bool found_slow_down_points{false}; + + bool stop_require{false}; + + bool slow_down_require{false}; + + bool enable_adaptive_cruise{false}; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/planner_utils.hpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/planner_utils.hpp new file mode 100644 index 0000000..75cea15 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/include/obstacle_stop_planner/planner_utils.hpp @@ -0,0 +1,178 @@ + +// Copyright 2022 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 OBSTACLE_STOP_PLANNER__PLANNER_UTILS_HPP_ +#define OBSTACLE_STOP_PLANNER__PLANNER_UTILS_HPP_ + +#include "obstacle_stop_planner/planner_data.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace motion_planning +{ + +namespace bg = boost::geometry; + +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using std_msgs::msg::Header; + +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; + +using TrajectoryPoints = std::vector; +using PointCloud = pcl::PointCloud; + +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE1) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE2) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE3) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja); + +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec); + +boost::optional> calcFeasibleMarginAndVelocity( + const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, + const double current_vel, const double current_acc); + +boost::optional> getForwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin); + +boost::optional> getBackwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin); + +boost::optional> findNearestFrontIndex( + const size_t start_idx, const TrajectoryPoints & trajectory, const Point & point); + +void insertStopPoint( + const StopPoint & stop_point, TrajectoryPoints & output, DiagnosticStatus & stop_reason_diag); + +bool isInFrontOfTargetPoint(const Pose & pose, const Point & point); + +bool checkValidIndex(const Pose & p_base, const Pose & p_next, const Pose & p_target); + +bool withinPolygon( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, + PointCloud::Ptr within_points_ptr); + +bool withinPolyhedron( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, + PointCloud::Ptr within_points_ptr, double z_min, double z_max); + +bool convexHull( + const std::vector & pointcloud, std::vector & polygon_points); + +void createOneStepPolygon( + const Pose & base_step_pose, const Pose & next_step_pose, std::vector & polygon, + const VehicleInfo & vehicle_info, const double expand_width = 0.0); + +bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose); + +void getNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * nearest_collision_point, + rclcpp::Time * nearest_collision_point_time); + +void getLateralNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, + double * deviation); + +Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); + +std::string jsonDumpsPose(const Pose & pose); + +DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose); + +TrajectoryPoint getBackwardPointFromBasePoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, + const double backward_length); + +TrajectoryPoints decimateTrajectory( + const TrajectoryPoints & input, const double step_length, std::map & index_map); + +TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double extend_distance); + +TrajectoryPoint getExtendTrajectoryPoint( + double extend_distance, const TrajectoryPoint & goal_point); + +rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__PLANNER_UTILS_HPP_ diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/launch/obstacle_stop_planner.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/launch/obstacle_stop_planner.launch.xml new file mode 100644 index 0000000..ed59659 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/launch/obstacle_stop_planner.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/package.xml new file mode 100644 index 0000000..97fc0cd --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/package.xml @@ -0,0 +1,47 @@ + + + + obstacle_stop_planner_custom + 0.1.0 + The obstacle_stop_planner package + + Satoshi Ota + + Apache License 2.0 + + Satoshi Ota + Yukihiro Saito + + ament_cmake_auto + + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + diagnostic_msgs + motion_utils + nav_msgs + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + signal_processing + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/adaptive_cruise_control.cpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/adaptive_cruise_control.cpp new file mode 100644 index 0000000..08a412c --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/adaptive_cruise_control.cpp @@ -0,0 +1,702 @@ +// 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 "obstacle_stop_planner/adaptive_cruise_control.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include +#include + +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +namespace +{ +Point convertPointRosToBoost(const geometry_msgs::msg::Point & point) +{ + const Point point2d(point.x, point.y); + return point2d; +} + +geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion quat(q.x, q.y, q.z, q.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + geometry_msgs::msg::Vector3 rpy; + rpy.x = roll; + rpy.y = pitch; + rpy.z = yaw; + return rpy; +} + +Polygon getPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, + const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) +{ + Polygon obj_poly; + geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); + + double l = size.x * std::cos(obj_rpy.y) + l_margin; + double w = size.y * std::cos(obj_rpy.x) + w_margin; + double co = center_offset; + bg::exterior_ring(obj_poly) = boost::assign::list_of(l / 2.0 + co, w / 2.0)( + -l / 2.0 + co, w / 2.0)(-l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); + + // rotate polygon + bg::strategy::transform::rotate_transformer rotate( + -obj_rpy.z); // original:clockwise rotation + Polygon rotate_obj_poly; + bg::transform(obj_poly, rotate_obj_poly, rotate); + // translate polygon + bg::strategy::transform::translate_transformer translate( + pose.position.x, pose.position.y); + Polygon translate_obj_poly; + bg::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +double getDistanceFromTwoPoint( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2) +{ + const double dx = point1.x - point2.x; + const double dy = point1.y - point2.y; + const double dist = std::hypot(dx, dy); + return dist; +} + +[[maybe_unused]] double normalizeRadian( + const double rad, const double min_rad = -boost::math::constants::pi(), + const double max_rad = boost::math::constants::pi()) +{ + const auto value = std::fmod(rad, 2 * boost::math::constants::pi()); + if (min_rad < value && value <= max_rad) { + return value; + } else { + return value - std::copysign(2 * boost::math::constants::pi(), value); + } +} + +constexpr double sign(const double value) +{ + if (value > 0) { + return 1.0; + } else if (value < 0) { + return -1.0; + } else { + return 0.0; + } +} +} // namespace + +namespace motion_planning +{ +AdaptiveCruiseController::AdaptiveCruiseController( + rclcpp::Node * node, const double vehicle_width, const double vehicle_length, + const double baselink2front) +: node_(node), + vehicle_width_(vehicle_width), + vehicle_length_(vehicle_length), + baselink2front_(baselink2front) +{ + // get parameter + std::string acc_ns = "adaptive_cruise_control."; + + /* config */ + param_.use_object_to_est_vel = + node_->declare_parameter(acc_ns + "use_object_to_estimate_vel", true); + param_.use_pcl_to_est_vel = node_->declare_parameter(acc_ns + "use_pcl_to_estimate_vel", true); + param_.consider_obj_velocity = node_->declare_parameter(acc_ns + "consider_obj_velocity", true); + + /* parameter for acc */ + param_.obstacle_velocity_thresh_to_start_acc = + node_->declare_parameter(acc_ns + "obstacle_velocity_thresh_to_start_acc", 1.5); + param_.obstacle_velocity_thresh_to_stop_acc = + node_->declare_parameter(acc_ns + "obstacle_velocity_thresh_to_stop_acc", 1.0); + param_.emergency_stop_acceleration = + node_->declare_parameter(acc_ns + "emergency_stop_acceleration", -3.5); + param_.obstacle_emergency_stop_acceleration = + node_->declare_parameter(acc_ns + "obstacle_emergency_stop_acceleration", -5.0); + param_.emergency_stop_idling_time = + node_->declare_parameter(acc_ns + "emergency_stop_idling_time", 0.5); + param_.min_dist_stop = node_->declare_parameter(acc_ns + "min_dist_stop", 4.0); + param_.max_standard_acceleration = + node_->declare_parameter(acc_ns + "max_standard_acceleration", 0.5); + param_.min_standard_acceleration = + node_->declare_parameter(acc_ns + "min_standard_acceleration", -1.0); + param_.standard_idling_time = node_->declare_parameter(acc_ns + "standard_idling_time", 0.5); + param_.min_dist_standard = node_->declare_parameter(acc_ns + "min_dist_standard", 4.0); + param_.obstacle_min_standard_acceleration = + node_->declare_parameter(acc_ns + "obstacle_min_standard_acceleration", -1.5); + param_.margin_rate_to_change_vel = + node_->declare_parameter(acc_ns + "margin_rate_to_change_vel", 0.3); + param_.use_time_compensation_to_dist = + node_->declare_parameter(acc_ns + "use_time_compensation_to_calc_distance", true); + param_.lowpass_gain_ = node_->declare_parameter(acc_ns + "lowpass_gain_of_upper_velocity", 0.6); + + /* parameter for pid in acc */ + param_.p_coeff_pos = node_->declare_parameter(acc_ns + "p_coefficient_positive", 0.1); + param_.p_coeff_neg = node_->declare_parameter(acc_ns + "p_coefficient_negative", 0.3); + param_.d_coeff_pos = node_->declare_parameter(acc_ns + "d_coefficient_positive", 0.0); + param_.d_coeff_neg = node_->declare_parameter(acc_ns + "d_coefficient_negative", 0.1); + + /* parameter for speed estimation of obstacle */ + param_.object_polygon_length_margin = + node_->declare_parameter(acc_ns + "object_polygon_length_margin", 2.0); + param_.object_polygon_width_margin = + node_->declare_parameter(acc_ns + "object_polygon_width_margin", 0.5); + param_.valid_est_vel_diff_time = + node_->declare_parameter(acc_ns + "valid_estimated_vel_diff_time", 1.0); + param_.valid_vel_que_time = node_->declare_parameter(acc_ns + "valid_vel_que_time", 0.5); + param_.valid_est_vel_max = node_->declare_parameter(acc_ns + "valid_estimated_vel_max", 20.0); + param_.valid_est_vel_min = node_->declare_parameter(acc_ns + "valid_estimated_vel_min", -20.0); + param_.thresh_vel_to_stop = node_->declare_parameter(acc_ns + "thresh_vel_to_stop", 0.5); + param_.use_rough_est_vel = + node_->declare_parameter(acc_ns + "use_rough_velocity_estimation", false); + param_.rough_velocity_rate = node_->declare_parameter(acc_ns + "rough_velocity_rate", 0.9); + + /* publisher */ + pub_debug_ = node_->create_publisher( + "~/adaptive_cruise_control/debug_values", 1); +} + +void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header) +{ + debug_values_.data.clear(); + debug_values_.data.resize(num_debug_values_, 0.0); + + const double current_velocity = current_velocity_ptr->twist.twist.linear.x; + double col_point_distance; + double point_velocity; + bool success_estimate_vel = false; + /* + * calc distance to collision point + */ + calcDistanceToNearestPointOnPath( + trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point, + nearest_collision_point_time, &col_point_distance, trajectory_header); + + /* + * calc yaw of trajectory at collision point + */ + const double traj_yaw = calcTrajYaw(trajectory, nearest_collision_point_idx); + + /* + * estimate velocity of collision point + */ + if (param_.use_pcl_to_est_vel) { + if (estimatePointVelocityFromPcl( + traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity)) { + success_estimate_vel = true; + } + } + + if (param_.use_object_to_est_vel) { + if (estimatePointVelocityFromObject( + object_ptr, traj_yaw, nearest_collision_point, &point_velocity)) { + success_estimate_vel = true; + } + } + + if (param_.use_rough_est_vel && !success_estimate_vel) { + point_velocity = estimateRoughPointVelocity(current_velocity); + success_estimate_vel = true; + } + + if (!success_estimate_vel) { + // if failed to estimate velocity, need to stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Failed to estimate velocity of forward vehicle. Insert stop line."); + *need_to_stop = true; + prev_upper_velocity_ = current_velocity; // reset prev_upper_velocity + prev_target_velocity_ = 0.0; + pub_debug_->publish(debug_values_); + return; + } + + // calculate max(target) velocity of self + const double upper_velocity = + calcUpperVelocity(col_point_distance, point_velocity, current_velocity); + pub_debug_->publish(debug_values_); + + if (upper_velocity <= param_.thresh_vel_to_stop) { + // if upper velocity is too low, need to stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Upper velocity is too low. Insert stop line."); + *need_to_stop = true; + return; + } + + /* + * insert max velocity + */ + insertMaxVelocityToPath( + self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory); + *need_to_stop = false; +} + +void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( + const TrajectoryPoints & trajectory, const int nearest_point_idx, + const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * distance, + const std_msgs::msg::Header & trajectory_header) +{ + if (trajectory.size() == 0) { + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "input path is too short(size=0)"); + *distance = 0; + return; + } + + // get self polygon + geometry_msgs::msg::Vector3 self_size; + self_size.x = vehicle_length_; + self_size.y = vehicle_width_; + double self_offset = baselink2front_ - vehicle_length_ / 2.0; + Polygon self_poly = getPolygon(self_pose, self_size, self_offset); + + // get nearest point + Point nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); + + if (nearest_point_idx <= 2) { + // if too nearest collision point, return direct distance + *distance = boost::geometry::distance(self_poly, nearest_point2d); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; + return; + } + + /* get total distance to collision point */ + double dist_to_point = 0; + // get distance from self to next nearest point + dist_to_point += boost::geometry::distance( + convertPointRosToBoost(self_pose.position), + convertPointRosToBoost(trajectory.at(1).pose.position)); + + // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx + for (int i = 1; i < nearest_point_idx - 1; i++) { + dist_to_point += + getDistanceFromTwoPoint(trajectory.at(i).pose.position, trajectory.at(i + 1).pose.position); + } + + // add distance from nearest_collision_point to prev point of nearest_point_idx + dist_to_point += boost::geometry::distance( + nearest_point2d, convertPointRosToBoost(trajectory.at(nearest_point_idx - 1).pose.position)); + + // subtract base_link to front + dist_to_point -= baselink2front_; + + // time compensation + if (param_.use_time_compensation_to_dist) { + const rclcpp::Time base_time = trajectory_header.stamp; + double delay_time = (base_time - nearest_collision_point_time).seconds(); + dist_to_point += prev_target_velocity_ * delay_time; + } + + *distance = std::max(0.0, dist_to_point); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; +} + +double AdaptiveCruiseController::calcTrajYaw( + const TrajectoryPoints & trajectory, const int collision_point_idx) +{ + return tf2::getYaw(trajectory.at(collision_point_idx).pose.orientation); +} + +bool AdaptiveCruiseController::estimatePointVelocityFromObject( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity) +{ + geometry_msgs::msg::Point nearest_collision_p_ros; + nearest_collision_p_ros.x = nearest_collision_point.x; + nearest_collision_p_ros.y = nearest_collision_point.y; + nearest_collision_p_ros.z = nearest_collision_point.z; + + /* get object velocity, and current yaw */ + bool get_obj = false; + double obj_vel; + double obj_yaw; + const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); + for (const auto & obj : object_ptr->objects) { + const Polygon obj_poly = getPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0, + param_.object_polygon_length_margin, param_.object_polygon_width_margin); + if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { + obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; + obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + get_obj = true; + break; + } + } + + if (get_obj) { + *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; + return true; + } else { + return false; + } +} + +bool AdaptiveCruiseController::estimatePointVelocityFromPcl( + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * velocity) +{ + geometry_msgs::msg::Point nearest_collision_p_ros; + nearest_collision_p_ros.x = nearest_collision_point.x; + nearest_collision_p_ros.y = nearest_collision_point.y; + nearest_collision_p_ros.z = nearest_collision_point.z; + + /* estimate velocity */ + const double p_dt = nearest_collision_point_time.seconds() - prev_collision_point_time_.seconds(); + + // if get same pointcloud with previous step, + // skip estimate process + if (std::fabs(p_dt) > std::numeric_limits::epsilon()) { + // valid time check + if (p_dt < 0 || param_.valid_est_vel_diff_time < p_dt) { + prev_collision_point_time_ = nearest_collision_point_time; + prev_collision_point_ = nearest_collision_point; + prev_collision_point_valid_ = true; + return false; + } + const double p_dx = nearest_collision_point.x - prev_collision_point_.x; + const double p_dy = nearest_collision_point.y - prev_collision_point_.y; + const double p_dist = std::hypot(p_dx, p_dy); + const double p_yaw = std::atan2(p_dy, p_dx); + const double p_vel = p_dist / p_dt; + const double est_velocity = p_vel * std::cos(p_yaw - traj_yaw); + // valid velocity check + if (est_velocity <= param_.valid_est_vel_min || param_.valid_est_vel_max <= est_velocity) { + prev_collision_point_time_ = nearest_collision_point_time; + prev_collision_point_ = nearest_collision_point; + prev_collision_point_valid_ = true; + est_vel_que_.clear(); + return false; + } + + // append new velocity and remove old velocity from que + registerQueToVelocity(est_velocity, nearest_collision_point_time); + } + + // calc average(median) velocity from que + *velocity = getMedianVel(est_vel_que_); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = *velocity; + + prev_collision_point_time_ = nearest_collision_point_time; + prev_collision_point_ = nearest_collision_point; + prev_target_velocity_ = *velocity; + prev_collision_point_valid_ = true; + return true; +} + +double AdaptiveCruiseController::estimateRoughPointVelocity(double current_vel) +{ + const double p_dt = node_->now().seconds() - prev_collision_point_time_.seconds(); + if (param_.valid_est_vel_diff_time >= p_dt) { + // use previous estimated velocity + return prev_target_velocity_; + } + + // use current velocity * rough velocity rate + return current_vel * param_.rough_velocity_rate; +} + +bool AdaptiveCruiseController::isObstacleVelocityHigh(const double obj_vel) +{ + bool is_high = false; + if (prev_obstacle_velocity_judge_to_start_acc_) { + is_high = obj_vel > param_.obstacle_velocity_thresh_to_stop_acc; + } else { + is_high = obj_vel > param_.obstacle_velocity_thresh_to_start_acc; + } + prev_obstacle_velocity_judge_to_start_acc_ = is_high; + return is_high; +} + +double AdaptiveCruiseController::calcUpperVelocity( + const double dist_to_col, const double obj_vel, const double self_vel) +{ + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_FINAL) = obj_vel; + if (!isObstacleVelocityHigh(obj_vel)) { + // stop acc by low-velocity obstacle + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "The velocity of forward vehicle is too low. Insert stop line."); + prev_upper_velocity_ = self_vel; // reset prev_upper_velocity + return 0.0; + } + + const double thresh_dist = calcThreshDistToForwardObstacle(self_vel, obj_vel); + if (thresh_dist >= dist_to_col) { + // emergency stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Forward vehicle is too close. Insert stop line."); + prev_upper_velocity_ = self_vel; // reset prev_upper_velocity + return 0.0; + } + + const double upper_velocity = + std::max(1e-01, calcTargetVelocityByPID(self_vel, dist_to_col, obj_vel)); + const double lowpass_upper_velocity = + lowpass_filter(upper_velocity, prev_upper_velocity_, param_.lowpass_gain_); + prev_upper_velocity_ = lowpass_upper_velocity; + debug_values_.data.at(DBGVAL::UPPER_VEL) = lowpass_upper_velocity; + return lowpass_upper_velocity; +} + +double AdaptiveCruiseController::calcThreshDistToForwardObstacle( + const double current_vel, const double obj_vel) +{ + const double current_vel_min = std::max(1.0, std::fabs(current_vel)); + const double obj_vel_min = std::max(0.0, obj_vel); + const double minimum_distance = param_.min_dist_stop; + const double idling_distance = current_vel_min * param_.emergency_stop_idling_time; + const double braking_distance = + (-1.0 * current_vel_min * current_vel_min) / (2.0 * param_.emergency_stop_acceleration); + const double obj_braking_distance = + (-1.0 * obj_vel_min * obj_vel_min) / (2.0 * param_.obstacle_emergency_stop_acceleration); + + return minimum_distance + std::max( + 0.0, idling_distance + braking_distance - + obj_braking_distance * param_.consider_obj_velocity); +} + +double AdaptiveCruiseController::calcBaseDistToForwardObstacle( + const double current_vel, const double obj_vel) +{ + const double obj_vel_min = std::max(0.0, obj_vel); + const double minimum_distance = param_.min_dist_standard; + const double idling_distance = current_vel * param_.standard_idling_time; + const double braking_distance = + (-1.0 * current_vel * current_vel) / (2.0 * param_.min_standard_acceleration); + const double obj_braking_distance = + (-1.0 * obj_vel_min * obj_vel_min) / (2.0 * param_.obstacle_min_standard_acceleration); + return minimum_distance + std::max( + 0.0, idling_distance + braking_distance - + obj_braking_distance * param_.consider_obj_velocity); +} + +double AdaptiveCruiseController::calcTargetVelocity_P( + const double target_dist, const double current_dist) +{ + const double diff_dist = current_dist - target_dist; + double add_vel_p; + if (diff_dist >= 0) { + add_vel_p = diff_dist * param_.p_coeff_pos; + } else { + add_vel_p = diff_dist * param_.p_coeff_neg; + } + return add_vel_p; +} + +double AdaptiveCruiseController::calcTargetVelocity_I( + [[maybe_unused]] const double target_dist, [[maybe_unused]] const double current_dist) +{ + // not implemented + return 0.0; +} + +double AdaptiveCruiseController::calcTargetVelocity_D( + const double target_dist, const double current_dist) +{ + if (node_->now().seconds() - prev_target_vehicle_time_ >= param_.d_coeff_valid_time) { + // invalid time(prev is too old) + return 0.0; + } + + double diff_vel = (target_dist - prev_target_vehicle_dist_) / + (node_->now().seconds() - prev_target_vehicle_time_); + + if (std::fabs(diff_vel) >= param_.d_coeff_valid_diff_vel) { + // invalid(discontinuous) diff_vel + return 0.0; + } + + double add_vel_d = 0; + + add_vel_d = diff_vel; + if (add_vel_d >= 0) { + diff_vel *= param_.d_coeff_pos; + } + if (add_vel_d < 0) { + diff_vel *= param_.d_coeff_neg; + } + add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm); + + // add buffer + prev_target_vehicle_dist_ = current_dist; + prev_target_vehicle_time_ = node_->now().seconds(); + + return add_vel_d; +} + +double AdaptiveCruiseController::calcTargetVelocityByPID( + const double current_vel, const double current_dist, const double obj_vel) +{ + const double target_dist = calcBaseDistToForwardObstacle(current_vel, obj_vel); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "[adaptive cruise control] target_dist" << target_dist); + + const double add_vel_p = calcTargetVelocity_P(target_dist, current_dist); + //** I is not implemented ** + const double add_vel_i = calcTargetVelocity_I(target_dist, current_dist); + const double add_vel_d = calcTargetVelocity_D(target_dist, current_dist); + + double target_vel = current_vel + add_vel_p + add_vel_i + add_vel_d; + debug_values_.data.at(DBGVAL::CURRENT_VEL) = current_vel; + debug_values_.data.at(DBGVAL::UPPER_VEL_P) = add_vel_p; + debug_values_.data.at(DBGVAL::UPPER_VEL_I) = add_vel_i; + debug_values_.data.at(DBGVAL::UPPER_VEL_D) = add_vel_d; + debug_values_.data.at(DBGVAL::UPPER_VEL_RAW) = target_vel; + return target_vel; +} + +void AdaptiveCruiseController::insertMaxVelocityToPath( + const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, + const double dist_to_collision_point, TrajectoryPoints * output_trajectory) +{ + // plus distance from self to next nearest point + double dist = dist_to_collision_point; + double dist_to_first_point = 0.0; + if (output_trajectory->size() > 1) { + dist_to_first_point = boost::geometry::distance( + convertPointRosToBoost(self_pose.position), + convertPointRosToBoost(output_trajectory->at(1).pose.position)); + } + dist += dist_to_first_point; + + double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; + // accel = (v_after^2 - v_before^2 ) / 2x + double target_acc = (std::pow(target_vel, 2) - std::pow(current_vel, 2)) / (2 * margin_to_insert); + + const double clipped_acc = boost::algorithm::clamp( + target_acc, param_.min_standard_acceleration, param_.max_standard_acceleration); + double pre_vel = current_vel; + double total_dist = 0.0; + for (size_t i = 1; i < output_trajectory->size(); i++) { + // calc velocity of each point by gradient deceleration + const auto current_p = output_trajectory->at(i); + const auto prev_p = output_trajectory->at(i - 1); + const double p_dist = getDistanceFromTwoPoint(current_p.pose.position, prev_p.pose.position); + total_dist += p_dist; + if (current_p.longitudinal_velocity_mps > target_vel && total_dist >= 0) { + double next_pre_vel; + if (std::fabs(clipped_acc) < 1e-05) { + next_pre_vel = pre_vel; + } else { + // v_after = sqrt (2x*accel + v_before^2) + next_pre_vel = std::sqrt(2 * p_dist * clipped_acc + std::pow(pre_vel, 2)); + } + if (target_acc >= 0) { + next_pre_vel = std::min(next_pre_vel, target_vel); + } else { + next_pre_vel = std::max(next_pre_vel, target_vel); + } + + if (total_dist >= margin_to_insert) { + const double max_velocity = std::max(target_vel, next_pre_vel); + if (output_trajectory->at(i).longitudinal_velocity_mps > max_velocity) { + output_trajectory->at(i).longitudinal_velocity_mps = max_velocity; + } + } + pre_vel = next_pre_vel; + } + } +} + +void AdaptiveCruiseController::registerQueToVelocity( + const double vel, const rclcpp::Time & vel_time) +{ + // remove old msg from que + std::vector delete_idxs; + for (size_t i = 0; i < est_vel_que_.size(); i++) { + if (node_->now().seconds() - est_vel_que_.at(i).header.stamp.sec > param_.valid_vel_que_time) { + delete_idxs.push_back(i); + } + } + for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { + est_vel_que_.erase(est_vel_que_.begin() + delete_idxs.at(delete_idx)); + } + + // append new que + nav_msgs::msg::Odometry new_vel; + new_vel.header.stamp = vel_time; + new_vel.twist.twist.linear.x = vel; + est_vel_que_.emplace_back(new_vel); +} + +double AdaptiveCruiseController::getMedianVel(const std::vector vel_que) +{ + if (vel_que.size() == 0) { + RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong."); + return 0.0; + } + + std::vector raw_vel_que; + for (const auto & vel : vel_que) { + raw_vel_que.emplace_back(vel.twist.twist.linear.x); + } + + double med_vel; + if (raw_vel_que.size() % 2 == 0) { + size_t med1 = (raw_vel_que.size()) / 2 - 1; + size_t med2 = (raw_vel_que.size()) / 2; + std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med1, raw_vel_que.end()); + const double vel1 = raw_vel_que[med1]; + std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med2, raw_vel_que.end()); + const double vel2 = raw_vel_que[med2]; + med_vel = (vel1 + vel2) / 2; + } else { + size_t med = (raw_vel_que.size() - 1) / 2; + std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med, raw_vel_que.end()); + med_vel = raw_vel_que[med]; + } + + return med_vel; +} + +double AdaptiveCruiseController::lowpass_filter( + const double current_value, const double prev_value, const double gain) +{ + return gain * prev_value + (1.0 - gain) * current_value; +} + +} // namespace motion_planning diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/debug_marker.cpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/debug_marker.cpp new file mode 100644 index 0000000..f53c71d --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/debug_marker.cpp @@ -0,0 +1,495 @@ +// 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 "obstacle_stop_planner/debug_marker.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +using motion_utils::createDeletedSlowDownVirtualWallMarker; +using motion_utils::createDeletedStopVirtualWallMarker; +using motion_utils::createSlowDownVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; + +namespace motion_planning +{ +ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( + rclcpp::Node * node, const double base_link2front) +: node_(node), base_link2front_(base_link2front) +{ + virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); + debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); + stop_reason_pub_ = node_->create_publisher("~/output/stop_reasons", 1); + pub_debug_values_ = + node_->create_publisher("~/obstacle_stop/debug_values", 1); +} + +bool ObstacleStopPlannerDebugNode::pushPolygon( + const std::vector & polygon, const double z, const PolygonType & type) +{ + std::vector eigen_polygon; + for (const auto & point : polygon) { + Eigen::Vector3d eigen_point; + eigen_point << point.x, point.y, z; + eigen_polygon.push_back(eigen_point); + } + return pushPolygon(eigen_polygon, type); +} + +bool ObstacleStopPlannerDebugNode::pushPolygon( + const std::vector & polygon, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polygon.empty()) { + vehicle_polygons_.push_back(polygon); + } + return true; + case PolygonType::Collision: + if (!polygon.empty()) { + collision_polygons_.push_back(polygon); + } + return true; + case PolygonType::SlowDownRange: + if (!polygon.empty()) { + slow_down_range_polygons_.push_back(polygon); + } + return true; + case PolygonType::SlowDown: + if (!polygon.empty()) { + slow_down_polygons_.push_back(polygon); + } + return true; + default: + return false; + } +} + +bool ObstacleStopPlannerDebugNode::pushPolyhedron( + const std::vector & polyhedron, const double z_min, const double z_max, + const PolygonType & type) +{ + std::vector eigen_polyhedron; + for (const auto & point : polyhedron) { + Eigen::Vector3d eigen_point; + eigen_point << point.x, point.y, z_min; + eigen_polyhedron.push_back(eigen_point); + Eigen::Vector3d eigen_point1; + eigen_point1 << point.x, point.y, z_max; + eigen_polyhedron.push_back(eigen_point1); + } + + return pushPolyhedron(eigen_polyhedron, type); +} + +bool ObstacleStopPlannerDebugNode::pushPolyhedron( + const std::vector & polyhedron, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polyhedron.empty()) { + vehicle_polyhedrons_.push_back(polyhedron); + } + return true; + case PolygonType::Collision: + if (!polyhedron.empty()) { + collision_polyhedrons_.push_back(polyhedron); + } + return true; + default: + return false; + } +} + +bool ObstacleStopPlannerDebugNode::pushPose(const Pose & pose, const PoseType & type) +{ + switch (type) { + case PoseType::Stop: + stop_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::TargetStop: + target_stop_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::SlowDownStart: + slow_down_start_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::SlowDownEnd: + slow_down_end_pose_ptr_ = std::make_shared(pose); + return true; + default: + return false; + } +} + +bool ObstacleStopPlannerDebugNode::pushObstaclePoint( + const Point & obstacle_point, const PointType & type) +{ + switch (type) { + case PointType::Stop: + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + case PointType::SlowDown: + slow_down_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + default: + return false; + } +} + +bool ObstacleStopPlannerDebugNode::pushObstaclePoint( + const pcl::PointXYZ & obstacle_point, const PointType & type) +{ + Point ros_point; + ros_point.x = obstacle_point.x; + ros_point.y = obstacle_point.y; + ros_point.z = obstacle_point.z; + return pushObstaclePoint(ros_point, type); +} + +void ObstacleStopPlannerDebugNode::publish() +{ + /* publish debug marker for rviz */ + const auto virtual_wall_msg = makeVirtualWallMarker(); + virtual_wall_pub_->publish(virtual_wall_msg); + + /* publish debug marker for rviz */ + const auto visualization_msg = makeVisualizationMarker(); + debug_viz_pub_->publish(visualization_msg); + + /* publish stop reason for autoware api */ + const auto stop_reason_msg = makeStopReasonArray(); + stop_reason_pub_->publish(stop_reason_msg); + + // publish debug values + Float32MultiArrayStamped debug_msg{}; + debug_msg.stamp = node_->now(); + for (const auto & v : debug_values_.getValues()) { + debug_msg.data.push_back(v); + } + pub_debug_values_->publish(debug_msg); + + /* reset variables */ + vehicle_polygons_.clear(); + collision_polygons_.clear(); + slow_down_range_polygons_.clear(); + slow_down_polygons_.clear(); + vehicle_polyhedrons_.clear(); + collision_polyhedrons_.clear(); + stop_pose_ptr_ = nullptr; + slow_down_start_pose_ptr_ = nullptr; + slow_down_end_pose_ptr_ = nullptr; + stop_obstacle_point_ptr_ = nullptr; + slow_down_obstacle_point_ptr_ = nullptr; +} + +MarkerArray ObstacleStopPlannerDebugNode::makeVirtualWallMarker() +{ + MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + if (stop_pose_ptr_ != nullptr) { + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = createStopVirtualWallMarker(p, "obstacle on the path", current_time, 0); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (slow_down_start_pose_ptr_ != nullptr && stop_pose_ptr_ == nullptr) { + const auto p = calcOffsetPose(*slow_down_start_pose_ptr_, base_link2front_, 0.0, 0.0); + + { + const auto markers = + createSlowDownVirtualWallMarker(p, "obstacle beside the path", current_time, 0); + appendMarkerArray(markers, &msg); + } + + { + auto markers = createSlowDownVirtualWallMarker(p, "slow down\nstart", current_time, 1); + markers.markers.front().ns = "slow_down_start_virtual_wall"; + markers.markers.back().ns = "slow_down_start_factor_text"; + appendMarkerArray(markers, &msg); + } + } else { + const auto markers = createDeletedSlowDownVirtualWallMarker(current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (slow_down_end_pose_ptr_ != nullptr && stop_pose_ptr_ == nullptr) { + const auto p = calcOffsetPose(*slow_down_end_pose_ptr_, base_link2front_, 0.0, 0.0); + auto markers = createSlowDownVirtualWallMarker(p, "slow down\nend", current_time, 2); + markers.markers.front().ns = "slow_down_end_virtual_wall"; + markers.markers.back().ns = "slow_down_end_factor_text"; + appendMarkerArray(markers, &msg); + } + + return msg; +} + +MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() +{ + MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + // cube + if (!vehicle_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_cubes", 0, Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size() - 2; ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = vehicle_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = vehicle_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + if (!collision_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_cubes", 0, Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < collision_polyhedrons_.at(i).size() - 2; ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = collision_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = collision_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + // polygon + if (!vehicle_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_polygons", 0, Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + { + const auto & p = vehicle_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == vehicle_polygons_.at(i).size()) { + const auto & p = vehicle_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = vehicle_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!collision_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_polygons", 0, Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polygons_.size(); ++i) { + for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + { + const auto & p = collision_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == collision_polygons_.at(i).size()) { + const auto & p = collision_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = collision_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!slow_down_range_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_detection_polygons", 0, Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { + for (size_t j = 0; j < slow_down_range_polygons_.at(i).size(); ++j) { + { + const auto & p = slow_down_range_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == slow_down_range_polygons_.at(i).size()) { + const auto & p = slow_down_range_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = slow_down_range_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!slow_down_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_polygons", 0, Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { + for (size_t j = 0; j < slow_down_polygons_.at(i).size(); ++j) { + { + const auto & p = slow_down_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == slow_down_polygons_.at(i).size()) { + const auto & p = slow_down_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = slow_down_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (target_stop_pose_ptr_ != nullptr) { + const auto p = calcOffsetPose(*target_stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = + createStopVirtualWallMarker(p, "obstacle_stop_target_stop_line", current_time, 0); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_point", 0, Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_text", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + if (slow_down_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_obstacle_point", 0, Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = *slow_down_obstacle_point_ptr_; + msg.markers.push_back(marker); + } + + if (slow_down_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_obstacle_text", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose.position = *slow_down_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + return msg; +} + +StopReasonArray ObstacleStopPlannerDebugNode::makeStopReasonArray() +{ + // create header + Header header; + header.frame_id = "map"; + header.stamp = node_->now(); + + // create stop reason stamped + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; + StopFactor stop_factor; + + if (stop_pose_ptr_ != nullptr) { + stop_factor.stop_pose = *stop_pose_ptr_; + if (stop_obstacle_point_ptr_ != nullptr) { + stop_factor.stop_factor_points.emplace_back(*stop_obstacle_point_ptr_); + } + stop_reason_msg.stop_factors.emplace_back(stop_factor); + } + + // create stop reason array + StopReasonArray stop_reason_array; + stop_reason_array.header = header; + stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); + return stop_reason_array; +} + +} // namespace motion_planning diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/node.cpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/node.cpp new file mode 100644 index 0000000..fe7e985 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/node.cpp @@ -0,0 +1,1014 @@ +// 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 +#include +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "obstacle_stop_planner/node.hpp" +#include "obstacle_stop_planner/planner_utils.hpp" + +#include +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +namespace motion_planning +{ +using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcLongitudinalOffsetToSegment; +using motion_utils::calcSignedArcLength; +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getPose; +using tier4_autoware_utils::getRPY; + +ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("obstacle_stop_planner", node_options) +{ + // Vehicle Parameters + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + const auto & i = vehicle_info_; + + // Parameters + { + auto & p = node_param_; + p.enable_slow_down = declare_parameter("enable_slow_down"); + p.enable_z_axis_obstacle_filtering = + declare_parameter("enable_z_axis_obstacle_filtering"); + p.z_axis_filtering_buffer = declare_parameter("z_axis_filtering_buffer"); + p.max_velocity = declare_parameter("max_velocity"); + p.hunting_threshold = declare_parameter("hunting_threshold"); + p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + } + + { + auto & p = stop_param_; + const std::string ns = "stop_planner."; + + // params for stop position + p.max_longitudinal_margin = + declare_parameter(ns + "stop_position.max_longitudinal_margin"); + p.min_longitudinal_margin = + declare_parameter(ns + "stop_position.min_longitudinal_margin"); + p.hold_stop_margin_distance = + declare_parameter(ns + "stop_position.hold_stop_margin_distance"); + + // params for detection area + p.lateral_margin = declare_parameter(ns + "detection_area.lateral_margin"); + p.extend_distance = declare_parameter(ns + "detection_area.extend_distance"); + p.step_length = declare_parameter(ns + "detection_area.step_length"); + + // apply offset + p.max_longitudinal_margin += i.max_longitudinal_offset_m; + p.min_longitudinal_margin += i.max_longitudinal_offset_m; + p.stop_search_radius = + p.step_length + + std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0); + } + + { + auto & p = slow_down_param_; + const std::string ns = "slow_down_planner."; + + // common param + p.normal_min_jerk = declare_parameter("normal.min_jerk"); + p.normal_min_acc = declare_parameter("normal.min_acc"); + p.limit_min_jerk = declare_parameter("limit.min_jerk"); + p.limit_min_acc = declare_parameter("limit.min_acc"); + + // params for slow down section + p.longitudinal_forward_margin = + declare_parameter(ns + "slow_down_section.longitudinal_forward_margin"); + p.longitudinal_backward_margin = + declare_parameter(ns + "slow_down_section.longitudinal_backward_margin"); + p.min_longitudinal_forward_margin = + declare_parameter(ns + "slow_down_section.min_longitudinal_forward_margin"); + p.longitudinal_margin_span = + declare_parameter(ns + "slow_down_section.longitudinal_margin_span"); + + // params for detection area + p.lateral_margin = declare_parameter(ns + "detection_area.lateral_margin"); + + // params for target velocity + p.max_slow_down_velocity = + declare_parameter(ns + "target_velocity.max_slow_down_velocity"); + p.min_slow_down_velocity = + declare_parameter(ns + "target_velocity.min_slow_down_velocity"); + p.slow_down_velocity = declare_parameter(ns + "target_velocity.slow_down_velocity"); + + // consider jerk/dec constraints in slow down + p.consider_constraints = declare_parameter(ns + "consider_constraints"); + p.slow_down_min_jerk = declare_parameter(ns + "constraints.jerk_min_slow_down"); + p.jerk_start = declare_parameter(ns + "constraints.jerk_start"); + p.jerk_span = declare_parameter(ns + "constraints.jerk_span"); + + p.velocity_threshold_decel_complete = + declare_parameter(ns + "velocity_threshold_decel_complete"); + p.acceleration_threshold_decel_complete = + declare_parameter(ns + "acceleration_threshold_decel_complete"); + + // apply offset + p.longitudinal_forward_margin += i.max_longitudinal_offset_m; + p.min_longitudinal_forward_margin += i.wheel_base_m + i.front_overhang_m; + p.longitudinal_backward_margin += i.rear_overhang_m; + p.slow_down_search_radius = + stop_param_.step_length + + std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0); + } + + // Initializer + acc_controller_ = std::make_unique( + this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); + debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); + last_detect_time_slowdown_point_ = this->now(); + last_detect_time_collision_point_ = this->now(); + + // Publishers + pub_trajectory_ = this->create_publisher("~/output/trajectory", 1); + + pub_stop_reason_ = this->create_publisher("~/output/stop_reason", 1); + + pub_point_cloud_ = this->create_publisher("~/output/point_cloud/debug", 1); + + pub_clear_velocity_limit_ = this->create_publisher( + "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); + + pub_velocity_limit_ = this->create_publisher( + "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); + + // Subscribers + sub_point_cloud_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&ObstacleStopPlannerNode::onPointCloud, this, std::placeholders::_1), + createSubscriptionOptions(this)); + + sub_trajectory_ = this->create_subscription( + "~/input/trajectory", 1, + std::bind(&ObstacleStopPlannerNode::onTrigger, this, std::placeholders::_1), + createSubscriptionOptions(this)); + + sub_odometry_ = this->create_subscription( + "~/input/odometry", 1, + std::bind(&ObstacleStopPlannerNode::onOdometry, this, std::placeholders::_1), + createSubscriptionOptions(this)); + + sub_acceleration_ = this->create_subscription( + "~/input/acceleration", 1, + std::bind(&ObstacleStopPlannerNode::onAcceleration, this, std::placeholders::_1), + createSubscriptionOptions(this)); + + sub_dynamic_objects_ = this->create_subscription( + "~/input/objects", 1, + std::bind(&ObstacleStopPlannerNode::onDynamicObjects, this, std::placeholders::_1), + createSubscriptionOptions(this)); + + sub_expand_stop_range_ = this->create_subscription( + "~/input/expand_stop_range", 1, + std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), + createSubscriptionOptions(this)); +} + +void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) +{ + // mutex for obstacle_ros_pointcloud_ptr_ + // NOTE: *obstacle_ros_pointcloud_ptr_ is used + std::lock_guard lock(mutex_); + + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::VoxelGrid filter; + PointCloud::Ptr pointcloud_ptr(new PointCloud); + PointCloud::Ptr no_height_pointcloud_ptr(new PointCloud); + PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); + + pcl::fromROSMsg(*input_msg, *pointcloud_ptr); + if (!node_param_.enable_z_axis_obstacle_filtering) { + for (const auto & point : pointcloud_ptr->points) { + no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); + } + filter.setInputCloud(no_height_pointcloud_ptr); + filter.setLeafSize(0.05f, 0.05f, 100000.0f); + filter.filter(*no_height_filtered_pointcloud_ptr); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + } else { + pcl::toROSMsg(*pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + } + obstacle_ros_pointcloud_ptr_->header = input_msg->header; +} + +void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg) +{ + mutex_.lock(); + // NOTE: these variables must not be referenced for multithreading + const auto vehicle_info = vehicle_info_; + const auto stop_param = stop_param_; + const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_; + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; + const auto current_acceleration_ptr = current_acceleration_ptr_; + mutex_.unlock(); + + { + const auto waiting = [this](const auto & str) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), "waiting for %s ...", + str); + }; + + if (!object_ptr) { + waiting("perception object"); + return; + } + + if (!obstacle_ros_pointcloud_ptr) { + waiting("obstacle pointcloud"); + return; + } + + if (!current_velocity_ptr) { + waiting("current velocity"); + return; + } + + if (!current_acceleration_ptr) { + waiting("current acceleration"); + return; + } + + if (input_msg->points.empty()) { + return; + } + } + + const auto current_vel = current_velocity_ptr->twist.twist.linear.x; + const auto current_acc = current_acceleration_ptr->accel.accel.linear.x; + + // TODO(someone): support backward path + const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(input_msg->points); + is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; + if (!is_driving_forward_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Backward path is NOT supported. publish input as it is."); + pub_trajectory_->publish(*input_msg); + return; + } + + PlannerData planner_data{}; + + getSelfPose(input_msg->header, tf_buffer_, planner_data.current_pose); + + Trajectory output_trajectory = *input_msg; + TrajectoryPoints output_trajectory_points = + motion_utils::convertToTrajectoryPointArray(*input_msg); + + // trim trajectory from self pose + const auto base_trajectory = trimTrajectoryWithIndexFromSelfPose( + motion_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, + planner_data.trajectory_trim_index); + // extend trajectory to consider obstacles after the goal + const auto extend_trajectory = extendTrajectory(base_trajectory, stop_param.extend_distance); + // decimate trajectory for calculation cost + const auto decimate_trajectory = decimateTrajectory( + extend_trajectory, stop_param.step_length, planner_data.decimate_trajectory_index_map); + + // search obstacles within slow-down/collision area + searchObstacle( + decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info, + stop_param, obstacle_ros_pointcloud_ptr); + // insert slow-down-section/stop-point + insertVelocity( + output_trajectory_points, planner_data, input_msg->header, vehicle_info, current_acc, + current_vel, stop_param); + + const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; + const auto no_hunting = + (rclcpp::Time(input_msg->header.stamp) - last_detect_time_slowdown_point_).seconds() > + node_param_.hunting_threshold; + if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { + resetExternalVelocityLimit(current_acc, current_vel); + } + + auto trajectory = motion_utils::convertToTrajectory(output_trajectory_points); + publishDebugData(planner_data, current_acc, current_vel); + + trajectory.header = input_msg->header; + pub_trajectory_->publish(trajectory); +} + +void ObstacleStopPlannerNode::searchObstacle( + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr) +{ + // search candidate obstacle pointcloud + PointCloud::Ptr slow_down_pointcloud_ptr(new PointCloud); + PointCloud::Ptr obstacle_candidate_pointcloud_ptr(new PointCloud); + if (!searchPointcloudNearTrajectory( + decimate_trajectory, obstacle_ros_pointcloud_ptr, obstacle_candidate_pointcloud_ptr, + trajectory_header, vehicle_info, stop_param)) { + return; + } + + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + // create one step circle center for vehicle + const auto & p_front = decimate_trajectory.at(i).pose; + const auto & p_back = decimate_trajectory.at(i + 1).pose; + const auto z_axis_min = p_front.position.z + 0.3; + const auto z_axis_max = + p_front.position.z + vehicle_info.vehicle_height_m + node_param_.z_axis_filtering_buffer; + const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); + const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); + const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); + const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); + + if (node_param_.enable_slow_down) { + std::vector one_step_move_slow_down_range_polygon; + // create one step polygon for slow_down range + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, + slow_down_param_.lateral_margin); + debug_ptr_->pushPolygon( + one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); + + if (node_param_.enable_z_axis_obstacle_filtering) { + planner_data.found_slow_down_points = withinPolyhedron( + one_step_move_slow_down_range_polygon, slow_down_param_.slow_down_search_radius, + prev_center_point, next_center_point, obstacle_candidate_pointcloud_ptr, + slow_down_pointcloud_ptr, z_axis_min, z_axis_max); + } else { + planner_data.found_slow_down_points = withinPolygon( + one_step_move_slow_down_range_polygon, slow_down_param_.slow_down_search_radius, + prev_center_point, next_center_point, obstacle_candidate_pointcloud_ptr, + slow_down_pointcloud_ptr); + } + const auto found_first_slow_down_points = + planner_data.found_slow_down_points && !planner_data.slow_down_require; + + if (found_first_slow_down_points) { + // found nearest slow down obstacle + planner_data.decimate_trajectory_slow_down_index = i; + planner_data.slow_down_require = true; + getNearestPoint( + *slow_down_pointcloud_ptr, p_front, &planner_data.nearest_slow_down_point, + &planner_data.nearest_collision_point_time); + getLateralNearestPoint( + *slow_down_pointcloud_ptr, p_front, &planner_data.lateral_nearest_slow_down_point, + &planner_data.lateral_deviation); + + debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown); + debug_ptr_->pushPolygon( + one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); + + last_detect_time_slowdown_point_ = trajectory_header.stamp; + } + + } else { + slow_down_pointcloud_ptr = obstacle_candidate_pointcloud_ptr; + } + + { + std::vector one_step_move_vehicle_polygon; + // create one step polygon for vehicle + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.lateral_margin); + if (node_param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon, z_axis_min, z_axis_max, PolygonType::Vehicle); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, + PolygonType::Vehicle); + } + + PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + if (node_param_.enable_z_axis_obstacle_filtering) { + auto before_size = collision_pointcloud_ptr->size(); + planner_data.found_collision_points = withinPolyhedron( + one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, + next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr, z_axis_min, + z_axis_max); + auto after_size = collision_pointcloud_ptr->size(); + if ((after_size - before_size) > 0) { + auto obstacle_ros_pointcloud_debug_ptr = std::make_shared(); + pcl::toROSMsg(*collision_pointcloud_ptr, *obstacle_ros_pointcloud_debug_ptr); + obstacle_ros_pointcloud_debug_ptr->header.frame_id = trajectory_header.frame_id; + pub_point_cloud_->publish(*obstacle_ros_pointcloud_debug_ptr); + } + } else { + planner_data.found_collision_points = withinPolygon( + one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, + next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr); + } + + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; + getNearestPoint( + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); + + if (node_param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon, z_axis_min, z_axis_max, PolygonType::Collision); + } else { + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + } + + planner_data.stop_require = planner_data.found_collision_points; + + mutex_.lock(); + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; + mutex_.unlock(); + + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, + &planner_data.stop_require, &output, trajectory_header); + + if (planner_data.stop_require) { + last_detect_time_collision_point_ = trajectory_header.stamp; + } + + break; + } + } + } +} + +void ObstacleStopPlannerNode::insertVelocity( + TrajectoryPoints & output, PlannerData & planner_data, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, + const StopParam & stop_param) +{ + const auto & base_link2front = vehicle_info.max_longitudinal_offset_m; + const auto no_hunting_collision_point = + (rclcpp::Time(trajectory_header.stamp) - last_detect_time_collision_point_).seconds() > + node_param_.hunting_threshold; + + if (planner_data.stop_require) { + // insert stop point + const auto traj_end_idx = output.size() - 1; + const auto idx = planner_data.decimate_trajectory_index_map.at( + planner_data.decimate_trajectory_collision_index) + + planner_data.trajectory_trim_index; + const auto index_with_dist_remain = findNearestFrontIndex( + std::min(idx, traj_end_idx), output, + createPoint( + planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); + + if (index_with_dist_remain) { + const auto vehicle_idx = std::min(planner_data.trajectory_trim_index, traj_end_idx); + const auto dist_baselink_to_obstacle = + calcSignedArcLength(output, vehicle_idx, index_with_dist_remain.get().first); + + debug_ptr_->setDebugValues( + DebugValues::TYPE::COLLISION_OBSTACLE_DISTANCE, + dist_baselink_to_obstacle + index_with_dist_remain.get().second - base_link2front); + + const auto stop_point = searchInsertPoint( + index_with_dist_remain.get().first, output, index_with_dist_remain.get().second, + stop_param); + + const auto & ego_pose = planner_data.current_pose; + const size_t ego_seg_idx = findFirstNearestIndexWithSoftConstraints( + output, ego_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + + const double stop_point_distance = [&]() { + if (output.size() < 2) { + return 0.0; + } + + size_t stop_seg_idx = 0; + const double lon_offset = + calcLongitudinalOffsetToSegment(output, stop_point.index, getPoint(stop_point.point)); + if (lon_offset < 0) { + stop_seg_idx = std::max(static_cast(0), stop_point.index - 1); + } else { + stop_seg_idx = std::min(output.size() - 2, stop_point.index); + } + + return calcSignedArcLength( + output, ego_pose.position, ego_seg_idx, getPoint(stop_point.point), stop_seg_idx); + }(); + const auto is_stopped = current_vel < 0.01; + + const auto & ego_pos = planner_data.current_pose.position; + if (stop_point_distance < stop_param_.hold_stop_margin_distance && is_stopped) { + const auto ego_pos_on_path = calcLongitudinalOffsetPose(output, ego_pos, 0.0); + + if (ego_pos_on_path) { + StopPoint current_stop_pos{}; + current_stop_pos.index = findFirstNearestSegmentIndexWithSoftConstraints( + output, ego_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + current_stop_pos.point.pose = ego_pos_on_path.get(); + + insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag); + latest_stop_point_ = current_stop_pos; + + debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop); + debug_ptr_->pushPose(getPose(current_stop_pos.point), PoseType::Stop); + } + + } else { + insertStopPoint(stop_point, output, planner_data.stop_reason_diag); + latest_stop_point_ = stop_point; + + debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop); + debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); + } + } + } else if (!no_hunting_collision_point) { + if (latest_stop_point_) { + // update stop point index with the current trajectory + latest_stop_point_.get().index = findFirstNearestSegmentIndexWithSoftConstraints( + output, getPose(latest_stop_point_.get().point), node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag); + debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::TargetStop); + debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::Stop); + } + } + + const auto no_hunting_slowdown_point = + (rclcpp::Time(trajectory_header.stamp) - last_detect_time_slowdown_point_).seconds() > + node_param_.hunting_threshold; + + if (planner_data.slow_down_require) { + // insert slow down point + const auto traj_end_idx = output.size() - 1; + const auto idx = planner_data.decimate_trajectory_index_map.at( + planner_data.decimate_trajectory_slow_down_index); + const auto index_with_dist_remain = findNearestFrontIndex( + std::min(idx, traj_end_idx), output, + createPoint( + planner_data.nearest_slow_down_point.x, planner_data.nearest_slow_down_point.y, 0)); + + if (index_with_dist_remain) { + const auto vehicle_idx = std::min(planner_data.trajectory_trim_index, traj_end_idx); + const auto dist_baselink_to_obstacle = + calcSignedArcLength(output, vehicle_idx, index_with_dist_remain.get().first); + + debug_ptr_->setDebugValues( + DebugValues::TYPE::SLOWDOWN_OBSTACLE_DISTANCE, + dist_baselink_to_obstacle + index_with_dist_remain.get().second - base_link2front); + const auto slow_down_section = createSlowDownSection( + index_with_dist_remain.get().first, output, planner_data.lateral_deviation, + index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, + current_vel); + + if ( + (!latest_slow_down_section_ && + dist_baselink_to_obstacle + index_with_dist_remain.get().second < + vehicle_info.max_longitudinal_offset_m) || + !no_hunting_slowdown_point) { + latest_slow_down_section_ = slow_down_section; + } + + insertSlowDownSection(slow_down_section, output); + } + } else if (!no_hunting_slowdown_point) { + if (latest_slow_down_section_) { + insertSlowDownSection(latest_slow_down_section_.get(), output); + } + } + + if (node_param_.enable_slow_down && latest_slow_down_section_) { + // check whether ego is in slow down section or not + const auto & p_start = latest_slow_down_section_.get().start_point.pose.position; + const auto & p_end = latest_slow_down_section_.get().end_point.pose.position; + const auto reach_slow_down_start_point = + isInFrontOfTargetPoint(planner_data.current_pose, p_start); + const auto reach_slow_down_end_point = isInFrontOfTargetPoint(planner_data.current_pose, p_end); + const auto is_in_slow_down_section = reach_slow_down_start_point && !reach_slow_down_end_point; + const auto index_with_dist_remain = findNearestFrontIndex(0, output, p_end); + + if (is_in_slow_down_section && index_with_dist_remain) { + const auto end_insert_point_with_idx = getBackwardInsertPointFromBasePoint( + index_with_dist_remain.get().first, output, -index_with_dist_remain.get().second); + + SlowDownSection slow_down_section{}; + slow_down_section.slow_down_start_idx = 0; + slow_down_section.start_point = output.front(); + slow_down_section.slow_down_end_idx = end_insert_point_with_idx.get().first; + slow_down_section.end_point = end_insert_point_with_idx.get().second; + slow_down_section.velocity = set_velocity_limit_ ? std::numeric_limits::max() + : slow_down_param_.slow_down_velocity; + + insertSlowDownSection(slow_down_section, output); + } else if (no_hunting_slowdown_point) { + latest_slow_down_section_ = {}; + } + } + + if (output.size() >= 2) { + for (size_t i = 0; i < output.size() - 2; ++i) { + const auto & p_base = output.at(i).pose; + const auto & p_target = output.at(i + 1).pose; + const auto & p_next = output.at(i + 2).pose; + if (!checkValidIndex(p_base, p_next, p_target)) { + RCLCPP_ERROR(get_logger(), "detect bad index"); + } + } + } + + pub_stop_reason_->publish(planner_data.stop_reason_diag); +} + +void ObstacleStopPlannerNode::onExpandStopRange(const ExpandStopRange::ConstSharedPtr input_msg) +{ + // mutex for vehicle_info_, stop_param_ + std::lock_guard lock(mutex_); + + const auto & i = vehicle_info_; + stop_param_.lateral_margin = input_msg->expand_stop_range; + stop_param_.stop_search_radius = + stop_param_.step_length + + std::hypot(i.vehicle_width_m / 2.0 + stop_param_.lateral_margin, i.vehicle_length_m / 2.0); +} + +StopPoint ObstacleStopPlannerNode::searchInsertPoint( + const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain, + const StopParam & stop_param) +{ + const auto max_dist_stop_point = + createTargetPoint(idx, stop_param.max_longitudinal_margin, base_trajectory, dist_remain); + const auto min_dist_stop_point = + createTargetPoint(idx, stop_param.min_longitudinal_margin, base_trajectory, dist_remain); + + // check if stop point is already inserted by behavior planner + bool is_inserted_already_stop_point = false; + const double epsilon = 1e-3; + for (int j = max_dist_stop_point.index - 1; j < static_cast(idx); ++j) { + if (std::abs(base_trajectory.at(std::max(j, 0)).longitudinal_velocity_mps) < epsilon) { + is_inserted_already_stop_point = true; + break; + } + } + // insert stop point + StopPoint stop_point{}; + stop_point.index = + !is_inserted_already_stop_point ? max_dist_stop_point.index : min_dist_stop_point.index; + stop_point.point = + !is_inserted_already_stop_point ? max_dist_stop_point.point : min_dist_stop_point.point; + return stop_point; +} + +StopPoint ObstacleStopPlannerNode::createTargetPoint( + const int idx, const double margin, const TrajectoryPoints & base_trajectory, + const double dist_remain) +{ + const auto update_margin_from_vehicle = margin - dist_remain; + const auto insert_point_with_idx = + getBackwardInsertPointFromBasePoint(idx, base_trajectory, update_margin_from_vehicle); + + if (!insert_point_with_idx) { + // TODO(Satoshi Ota) + return StopPoint{}; + } + + StopPoint stop_point{}; + stop_point.index = insert_point_with_idx.get().first; + stop_point.point = insert_point_with_idx.get().second; + + return stop_point; +} + +SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( + const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, + const double dist_remain, const double dist_baselink_to_obstacle, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel) +{ + if (slow_down_param_.consider_constraints) { + const auto margin_with_vel = calcFeasibleMarginAndVelocity( + slow_down_param_, dist_baselink_to_obstacle + dist_remain, current_vel, current_acc); + + const auto relax_target_vel = margin_with_vel == boost::none; + if (relax_target_vel && !set_velocity_limit_) { + setExternalVelocityLimit(); + } + + const auto no_need_velocity_limit = + dist_baselink_to_obstacle + dist_remain > slow_down_param_.longitudinal_forward_margin; + if (set_velocity_limit_ && no_need_velocity_limit) { + resetExternalVelocityLimit(current_acc, current_vel); + } + + const auto use_velocity_limit = relax_target_vel || set_velocity_limit_; + + const auto update_forward_margin_from_vehicle = + use_velocity_limit ? slow_down_param_.min_longitudinal_forward_margin - dist_remain + : margin_with_vel.get().first - dist_remain; + const auto update_backward_margin_from_vehicle = + slow_down_param_.longitudinal_backward_margin + dist_remain; + + const auto velocity = + use_velocity_limit ? std::numeric_limits::max() : margin_with_vel.get().second; + + return createSlowDownSectionFromMargin( + idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, + velocity); + } else { + const auto update_forward_margin_from_vehicle = + slow_down_param_.longitudinal_forward_margin - dist_remain; + const auto update_backward_margin_from_vehicle = + slow_down_param_.longitudinal_backward_margin + dist_remain; + + const auto velocity = + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * + std::max(lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / + slow_down_param_.lateral_margin; + + return createSlowDownSectionFromMargin( + idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, + velocity); + } + + return SlowDownSection{}; +} + +SlowDownSection ObstacleStopPlannerNode::createSlowDownSectionFromMargin( + const int idx, const TrajectoryPoints & base_trajectory, const double forward_margin, + const double backward_margin, const double velocity) +{ + // calc slow down start point + const auto start_insert_point_with_idx = + getBackwardInsertPointFromBasePoint(idx, base_trajectory, forward_margin); + // calc slow down end point + const auto end_insert_point_with_idx = + getForwardInsertPointFromBasePoint(idx, base_trajectory, backward_margin); + + if (!start_insert_point_with_idx || !end_insert_point_with_idx) { + // TODO(Satoshi Ota) + return SlowDownSection{}; + } + + SlowDownSection slow_down_section{}; + slow_down_section.slow_down_start_idx = start_insert_point_with_idx.get().first; + slow_down_section.start_point = start_insert_point_with_idx.get().second; + slow_down_section.slow_down_end_idx = end_insert_point_with_idx.get().first; + slow_down_section.end_point = end_insert_point_with_idx.get().second; + slow_down_section.velocity = velocity; + + return slow_down_section; +} + +void ObstacleStopPlannerNode::insertSlowDownSection( + const SlowDownSection & slow_down_section, TrajectoryPoints & output) +{ + const auto traj_end_idx = output.size() - 1; + const auto & start_idx = slow_down_section.slow_down_start_idx; + const auto & end_idx = slow_down_section.slow_down_end_idx; + + const auto p_base_start = output.at(start_idx); + const auto p_next_start = output.at(std::min(start_idx + 1, traj_end_idx)); + const auto & p_insert_start = slow_down_section.start_point; + + const auto p_base_end = output.at(end_idx); + const auto p_next_end = output.at(std::min(end_idx + 1, traj_end_idx)); + const auto & p_insert_end = slow_down_section.end_point; + + constexpr double min_dist = 1e-3; + + const auto is_valid_index_start = + checkValidIndex(p_base_start.pose, p_next_start.pose, p_insert_start.pose); + const auto is_start_p_base_and_p_insert_overlap = + calcDistance2d(p_base_start, p_insert_start) < min_dist; + const auto is_start_p_next_and_p_insert_overlap = + calcDistance2d(p_next_start, p_insert_start) < min_dist; + + auto update_start_idx = start_idx; + auto update_end_idx = end_idx; + + if ( + !is_start_p_base_and_p_insert_overlap && !is_start_p_next_and_p_insert_overlap && + is_valid_index_start) { + // insert: start_idx and end_idx are shifted by one + output.insert(output.begin() + start_idx + 1, p_insert_start); + update_start_idx = std::min(update_start_idx + 1, traj_end_idx); + update_end_idx = std::min(update_end_idx + 1, traj_end_idx); + } else if (is_start_p_next_and_p_insert_overlap) { + // not insert: p_insert is merged into p_next + update_start_idx = std::min(update_start_idx + 1, traj_end_idx); + } + + const auto is_end_p_base_and_p_insert_overlap = + calcDistance2d(p_base_end, p_insert_end) < min_dist; + const auto is_end_p_next_and_p_insert_overlap = + calcDistance2d(p_next_end, p_insert_end) < min_dist; + const auto is_valid_index_end = + checkValidIndex(p_base_end.pose, p_next_end.pose, p_insert_end.pose); + + if ( + !is_end_p_base_and_p_insert_overlap && !is_end_p_next_and_p_insert_overlap && + is_valid_index_end) { + // insert: end_idx is shifted by one + output.insert(output.begin() + update_end_idx + 1, p_insert_end); + update_end_idx = std::min(update_end_idx + 1, traj_end_idx); + } else if (is_end_p_next_and_p_insert_overlap) { + // not insert: p_insert is merged into p_next + update_end_idx = std::min(update_end_idx + 1, traj_end_idx); + } + + for (size_t i = update_start_idx; i <= update_end_idx; ++i) { + output.at(i).longitudinal_velocity_mps = std::min( + slow_down_section.velocity, static_cast(output.at(i).longitudinal_velocity_mps)); + } + + debug_ptr_->pushPose(p_base_start.pose, PoseType::SlowDownStart); + debug_ptr_->pushPose(p_base_end.pose, PoseType::SlowDownEnd); +} + +void ObstacleStopPlannerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg) +{ + std::lock_guard lock(mutex_); + + object_ptr_ = input_msg; +} + +void ObstacleStopPlannerNode::onOdometry(const Odometry::ConstSharedPtr input_msg) +{ + // mutex for current_acc_, lpf_acc_ + std::lock_guard lock(mutex_); + current_velocity_ptr_ = input_msg; +} + +void ObstacleStopPlannerNode::onAcceleration( + const AccelWithCovarianceStamped::ConstSharedPtr input_msg) +{ + std::lock_guard lock(mutex_); + current_acceleration_ptr_ = input_msg; +} + +TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose, size_t & index) +{ + TrajectoryPoints output{}; + + const size_t min_distance_index = findFirstNearestIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + + for (size_t i = min_distance_index; i < input.size(); ++i) { + output.push_back(input.at(i)); + } + index = min_distance_index; + + return output; +} + +bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( + const TrajectoryPoints & trajectory, const PointCloud2::ConstSharedPtr & input_points_ptr, + PointCloud::Ptr output_points_ptr, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const StopParam & stop_param) +{ + // transform pointcloud + TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + trajectory_header.frame_id, input_points_ptr->header.frame_id, input_points_ptr->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << trajectory_header.frame_id << " to " + << input_points_ptr->header.frame_id); + return false; + } + + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *input_points_ptr, transformed_points); + PointCloud::Ptr transformed_points_ptr(new PointCloud); + pcl::fromROSMsg(transformed_points, *transformed_points_ptr); + + output_points_ptr->header = transformed_points_ptr->header; + + // search obstacle candidate pointcloud to reduce calculation cost + const double search_radius = node_param_.enable_slow_down + ? slow_down_param_.slow_down_search_radius + : stop_param.stop_search_radius; + const double squared_radius = search_radius * search_radius; + std::vector center_points; + center_points.reserve(trajectory.size()); + for (const auto & trajectory_point : trajectory) + center_points.push_back(getVehicleCenterFromBase(trajectory_point.pose, vehicle_info).position); + for (const auto & point : transformed_points_ptr->points) { + for (const auto & center_point : center_points) { + const double x = center_point.x - point.x; + const double y = center_point.y - point.y; + const double squared_distance = x * x + y * y; + if (squared_distance < squared_radius) { + output_points_ptr->points.push_back(point); + break; + } + } + } + return true; +} + +void ObstacleStopPlannerNode::setExternalVelocityLimit() +{ + const auto & p = slow_down_param_; + auto slow_down_limit_vel = std::make_shared(); + slow_down_limit_vel->stamp = this->now(); + slow_down_limit_vel->max_velocity = p.slow_down_velocity; + slow_down_limit_vel->constraints.min_acceleration = + p.slow_down_min_jerk < p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + slow_down_limit_vel->constraints.max_jerk = std::abs(p.slow_down_min_jerk); + slow_down_limit_vel->constraints.min_jerk = p.slow_down_min_jerk; + slow_down_limit_vel->use_constraints = true; + slow_down_limit_vel->sender = "obstacle_stop_planner"; + + pub_velocity_limit_->publish(*slow_down_limit_vel); + set_velocity_limit_ = true; + + RCLCPP_INFO( + get_logger(), "set velocity limit. jerk:%-6.2f dec:%-6.2f", + slow_down_limit_vel->constraints.min_jerk, slow_down_limit_vel->constraints.min_acceleration); +} + +void ObstacleStopPlannerNode::resetExternalVelocityLimit( + const double current_acc, const double current_vel) +{ + const auto reach_target_vel = current_vel < slow_down_param_.slow_down_velocity + + slow_down_param_.velocity_threshold_decel_complete; + const auto constant_vel = + std::abs(current_acc) < slow_down_param_.acceleration_threshold_decel_complete; + const auto no_undershoot = reach_target_vel && constant_vel; + + if (!no_undershoot) { + return; + } + + auto velocity_limit_clear_command = std::make_shared(); + velocity_limit_clear_command->stamp = this->now(); + velocity_limit_clear_command->command = true; + velocity_limit_clear_command->sender = "obstacle_stop_planner"; + + pub_clear_velocity_limit_->publish(*velocity_limit_clear_command); + set_velocity_limit_ = false; + + RCLCPP_INFO(get_logger(), "reset velocity limit"); +} + +void ObstacleStopPlannerNode::publishDebugData( + const PlannerData & planner_data, const double current_acc, const double current_vel) +{ + debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_VEL, current_vel); + debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_ACC, current_acc); + debug_ptr_->setDebugValues( + DebugValues::TYPE::FLAG_FIND_SLOW_DOWN_OBSTACLE, planner_data.slow_down_require); + debug_ptr_->setDebugValues( + DebugValues::TYPE::FLAG_FIND_COLLISION_OBSTACLE, planner_data.stop_require); + debug_ptr_->setDebugValues(DebugValues::TYPE::FLAG_EXTERNAL, set_velocity_limit_); + + const auto now_adaptive_cruise = + !planner_data.stop_require && planner_data.found_collision_points; + debug_ptr_->setDebugValues(DebugValues::TYPE::FLAG_ADAPTIVE_CRUISE, now_adaptive_cruise); + + debug_ptr_->publish(); +} + +} // namespace motion_planning + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(motion_planning::ObstacleStopPlannerNode) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/planner_utils.cpp b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/planner_utils.cpp new file mode 100644 index 0000000..2a503dc --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/obstacle_stop_planner_custom/src/planner_utils.cpp @@ -0,0 +1,739 @@ +// 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 "obstacle_stop_planner/planner_utils.hpp" + +#include +#include + +#include + +#include + +#include + +namespace motion_planning +{ + +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getRPY; + +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + + if (v_end < v_min || v_max < v_end) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! v_target = " << v_target << ", v_end = " << v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! a_target = " << a_target << ", a_end = " << a_end); + return false; + } + + return true; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE1) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min) +{ + constexpr double epsilon = 1e-3; + + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const double a1 = a0 + j1 * t1; + const double v1 = v0 + a0 * t1 + 0.5 * j1 * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * j1 * t1 * t1 * t1; + + const double t2 = epsilon < t_min ? t_min : 0.0; + const double a2 = a1; + const double v2 = v1 + a1 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; + + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const double a3 = a2 + ja * t3; + const double v3 = v2 + a2 * t3 + 0.5 * ja * t3 * t3; + const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * ja * t3 * t3 * t3; + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + + if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x3; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE2) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const double v1 = v0 + a0 * t1 + 0.5 * jd * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jd * t1 * t1 * t1; + + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const double a2 = a1 + ja * t2; + const double v2 = v1 + a1 * t2 + 0.5 * ja * t2 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * ja * t2 * t2 * t2; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x2; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE3) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + const double t_acc = (0.0 - a0) / ja; + + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const double a1 = a0 + ja * t1; + const double v1 = v0 + a0 * t1 + 0.5 * ja * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * ja * t1 * t1 * t1; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x1; +} + +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec) +{ + constexpr double epsilon = 1e-3; + const double t_dec = + acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; + const double t_acc = (0.0 - acc_min) / jerk_acc; + const double t_min = (target_vel - current_vel - current_acc * t_dec - + 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_min > epsilon) { + return calcDecelDistPlanType1( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); + } else if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } else { + return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); + } + + return {}; +} + +boost::optional> calcFeasibleMarginAndVelocity( + const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, + const double current_vel, const double current_acc) +{ + const auto & p = slow_down_param; + const auto & logger = rclcpp::get_logger("calcFeasibleMarginAndVelocity"); + constexpr double epsilon = 1e-4; + + if (current_vel < p.slow_down_velocity + epsilon) { + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); + } + + for (double planning_jerk = p.jerk_start; planning_jerk > p.slow_down_min_jerk - epsilon; + planning_jerk += p.jerk_span) { + const double jerk_dec = planning_jerk; + const double jerk_acc = std::abs(planning_jerk); + + const auto planning_dec = + planning_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); + + if (!stop_dist) { + continue; + } + + if (stop_dist.get() + p.longitudinal_forward_margin < dist_baselink_to_obstacle) { + RCLCPP_DEBUG( + logger, "[found plan] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt:%-6.2f", + stop_dist.get(), planning_jerk, p.longitudinal_forward_margin, p.slow_down_velocity, + current_vel); + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); + } + } + + { + const double jerk_dec = p.slow_down_min_jerk; + const double jerk_acc = std::abs(p.slow_down_min_jerk); + + const auto planning_dec = + p.slow_down_min_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); + + if (!stop_dist) { + return {}; + } + + if (stop_dist.get() + p.min_longitudinal_forward_margin < dist_baselink_to_obstacle) { + const auto planning_margin = dist_baselink_to_obstacle - stop_dist.get(); + RCLCPP_DEBUG( + logger, "[relax margin] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt%-6.2f", + stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_velocity, current_vel); + return std::make_pair(planning_margin, p.slow_down_velocity); + } + } + + RCLCPP_DEBUG(logger, "relax target slow down velocity"); + return {}; +} + +boost::optional> getForwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) +{ + if (base_idx + 1 > trajectory.size()) { + return {}; + } + + if (margin < std::numeric_limits::epsilon()) { + return std::make_pair(base_idx, trajectory.at(base_idx)); + } + + double length_sum = 0.0; + double length_residual = 0.0; + + for (size_t i = base_idx; i < trajectory.size() - 1; ++i) { + const auto & p_front = trajectory.at(i); + const auto & p_back = trajectory.at(i + 1); + + length_sum += calcDistance2d(p_front, p_back); + length_residual = length_sum - margin; + + if (length_residual >= std::numeric_limits::epsilon()) { + const auto p_insert = getBackwardPointFromBasePoint(p_back, p_front, p_back, length_residual); + + // p_front(trajectory.points.at(i)) is insert base point + return std::make_pair(i, p_insert); + } + } + + if (length_residual < std::numeric_limits::epsilon()) { + return std::make_pair(trajectory.size() - 1, trajectory.back()); + } + + return {}; +} + +boost::optional> getBackwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) +{ + if (base_idx + 1 > trajectory.size()) { + return {}; + } + + if (margin < std::numeric_limits::epsilon()) { + return std::make_pair(base_idx, trajectory.at(base_idx)); + } + + double length_sum = 0.0; + double length_residual = 0.0; + + for (size_t i = base_idx; 0 < i; --i) { + const auto & p_front = trajectory.at(i - 1); + const auto & p_back = trajectory.at(i); + + length_sum += calcDistance2d(p_front, p_back); + length_residual = length_sum - margin; + + if (length_residual >= std::numeric_limits::epsilon()) { + const auto p_insert = + getBackwardPointFromBasePoint(p_front, p_back, p_front, length_residual); + + // p_front(trajectory.points.at(i-1)) is insert base point + return std::make_pair(i - 1, p_insert); + } + } + + if (length_residual < std::numeric_limits::epsilon()) { + return std::make_pair(size_t(0), trajectory.front()); + } + + return {}; +} + +boost::optional> findNearestFrontIndex( + const size_t start_idx, const TrajectoryPoints & trajectory, const Point & point) +{ + for (size_t i = start_idx; i < trajectory.size(); ++i) { + const auto & p_traj = trajectory.at(i).pose; + const auto yaw = getRPY(p_traj).z; + const Point2d p_traj_direction(std::cos(yaw), std::sin(yaw)); + const Point2d p_traj_to_target(point.x - p_traj.position.x, point.y - p_traj.position.y); + + const auto is_in_front_of_target_point = p_traj_direction.dot(p_traj_to_target) < 0.0; + const auto is_trajectory_end = i + 1 == trajectory.size(); + + if (is_in_front_of_target_point || is_trajectory_end) { + const auto dist_p_traj_to_target = p_traj_direction.normalized().dot(p_traj_to_target); + return std::make_pair(i, dist_p_traj_to_target); + } + } + + return {}; +} + +bool isInFrontOfTargetPoint(const Pose & pose, const Point & point) +{ + const auto yaw = getRPY(pose).z; + const Point2d pose_direction(std::cos(yaw), std::sin(yaw)); + const Point2d to_target(point.x - pose.position.x, point.y - pose.position.y); + + return pose_direction.dot(to_target) < 0.0; +} + +bool checkValidIndex(const Pose & p_base, const Pose & p_next, const Pose & p_target) +{ + const Point2d base2target( + p_target.position.x - p_base.position.x, p_target.position.y - p_base.position.y); + const Point2d target2next( + p_next.position.x - p_target.position.x, p_next.position.y - p_target.position.y); + return base2target.dot(target2next) > 0.0; +} + +std::string jsonDumpsPose(const Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} + +DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose) +{ + DiagnosticStatus stop_reason_diag; + KeyValue stop_reason_diag_kv; + stop_reason_diag.level = DiagnosticStatus::OK; + stop_reason_diag.name = "stop_reason"; + stop_reason_diag.message = stop_reason; + stop_reason_diag_kv.key = "stop_pose"; + stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); + stop_reason_diag.values.push_back(stop_reason_diag_kv); + return stop_reason_diag; +} + +TrajectoryPoint getBackwardPointFromBasePoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, + const double backward_length) +{ + TrajectoryPoint output; + const double dx = p_to.pose.position.x - p_from.pose.position.x; + const double dy = p_to.pose.position.y - p_from.pose.position.y; + const double norm = std::hypot(dx, dy); + + output = p_base; + output.pose.position.x += backward_length * dx / norm; + output.pose.position.y += backward_length * dy / norm; + + return output; +} + +rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} + +bool withinPolygon( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, + PointCloud::Ptr within_points_ptr) +{ + Polygon2d boost_polygon; + bool find_within_points = false; + for (const auto & point : cv_polygon) { + boost_polygon.outer().push_back(bg::make(point.x, point.y)); + } + boost_polygon.outer().push_back(bg::make(cv_polygon.front().x, cv_polygon.front().y)); + + for (size_t j = 0; j < candidate_points_ptr->size(); ++j) { + Point2d point(candidate_points_ptr->at(j).x, candidate_points_ptr->at(j).y); + if (bg::distance(prev_point, point) < radius || bg::distance(next_point, point) < radius) { + if (bg::within(point, boost_polygon)) { + within_points_ptr->push_back(candidate_points_ptr->at(j)); + find_within_points = true; + } + } + } + return find_within_points; +} + +bool withinPolyhedron( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, + PointCloud::Ptr within_points_ptr, double z_min, double z_max) +{ + Polygon2d boost_polygon; + bool find_within_points = false; + for (const auto & point : cv_polygon) { + boost_polygon.outer().push_back(bg::make(point.x, point.y)); + } + boost_polygon.outer().push_back(bg::make(cv_polygon.front().x, cv_polygon.front().y)); + + for (size_t j = 0; j < candidate_points_ptr->size(); ++j) { + Point2d point(candidate_points_ptr->at(j).x, candidate_points_ptr->at(j).y); + if (bg::distance(prev_point, point) < radius || bg::distance(next_point, point) < radius) { + if (bg::within(point, boost_polygon)) { + if (candidate_points_ptr->at(j).z < z_max && candidate_points_ptr->at(j).z > z_min) { + within_points_ptr->push_back(candidate_points_ptr->at(j)); + find_within_points = true; + } + } + } + } + return find_within_points; +} + +bool convexHull( + const std::vector & pointcloud, std::vector & polygon_points) +{ + cv::Point2d centroid; + centroid.x = 0; + centroid.y = 0; + for (const auto & point : pointcloud) { + centroid.x += point.x; + centroid.y += point.y; + } + centroid.x = centroid.x / static_cast(pointcloud.size()); + centroid.y = centroid.y / static_cast(pointcloud.size()); + + std::vector normalized_pointcloud; + std::vector normalized_polygon_points; + for (const auto & p : pointcloud) { + normalized_pointcloud.emplace_back( + cv::Point((p.x - centroid.x) * 1000.0, (p.y - centroid.y) * 1000.0)); + } + cv::convexHull(normalized_pointcloud, normalized_polygon_points); + + for (const auto & p : normalized_polygon_points) { + cv::Point2d polygon_point; + polygon_point.x = (p.x / 1000.0 + centroid.x); + polygon_point.y = (p.y / 1000.0 + centroid.y); + polygon_points.push_back(polygon_point); + } + return true; +} + +void createOneStepPolygon( + const Pose & base_step_pose, const Pose & next_step_pose, std::vector & polygon, + const VehicleInfo & vehicle_info, const double expand_width) +{ + std::vector one_step_move_vehicle_corner_points; + + const auto & i = vehicle_info; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0 + expand_width; + const auto & back_m = i.rear_overhang_m; + // start step + { + const auto yaw = getRPY(base_step_pose).z; + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, + base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, + base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, + base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, + base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); + } + // next step + { + const auto yaw = getRPY(next_step_pose).z; + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, + next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, + next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, + next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, + next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); + } + convexHull(one_step_move_vehicle_corner_points, polygon); +} + +void insertStopPoint( + const StopPoint & stop_point, TrajectoryPoints & output, DiagnosticStatus & stop_reason_diag) +{ + const auto traj_end_idx = output.size() - 1; + const auto & stop_idx = stop_point.index; + + const auto & p_base = output.at(stop_idx); + const auto & p_next = output.at(std::min(stop_idx + 1, traj_end_idx)); + const auto & p_insert = stop_point.point; + + constexpr double min_dist = 1e-3; + + const auto is_p_base_and_p_insert_overlap = calcDistance2d(p_base, p_insert) < min_dist; + const auto is_p_next_and_p_insert_overlap = calcDistance2d(p_next, p_insert) < min_dist; + const auto is_valid_index = checkValidIndex(p_base.pose, p_next.pose, p_insert.pose); + + auto update_stop_idx = stop_idx; + + if (!is_p_base_and_p_insert_overlap && !is_p_next_and_p_insert_overlap && is_valid_index) { + // insert: start_idx and end_idx are shifted by one + output.insert(output.begin() + stop_idx + 1, p_insert); + update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); + } else if (is_p_next_and_p_insert_overlap) { + // not insert: p_insert is merged into p_next + update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); + } + + for (size_t i = update_stop_idx; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + } + + stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose); +} + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + +TrajectoryPoints decimateTrajectory( + const TrajectoryPoints & input, const double step_length, + std::map & index_map) +{ + TrajectoryPoints output{}; + + double trajectory_length_sum = 0.0; + double next_length = 0.0; + + for (int i = 0; i < static_cast(input.size()) - 1; ++i) { + const auto & p_front = input.at(i); + const auto & p_back = input.at(i + 1); + constexpr double epsilon = 1e-3; + + if (next_length <= trajectory_length_sum + epsilon) { + const auto p_interpolate = + getBackwardPointFromBasePoint(p_front, p_back, p_back, next_length - trajectory_length_sum); + output.push_back(p_interpolate); + + index_map.insert(std::make_pair(output.size() - 1, size_t(i))); + next_length += step_length; + continue; + } + + trajectory_length_sum += calcDistance2d(p_front, p_back); + } + if (!input.empty()) { + output.push_back(input.back()); + index_map.insert(std::make_pair(output.size() - 1, input.size() - 1)); + } + + return output; +} + +TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double extend_distance) +{ + TrajectoryPoints output = input; + + if (extend_distance < std::numeric_limits::epsilon()) { + return output; + } + + const auto goal_point = input.back(); + double interpolation_distance = 0.1; + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - interpolation_distance)) { + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + output.push_back(extend_trajectory_point); + extend_sum += interpolation_distance; + } + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + output.push_back(extend_trajectory_point); + + return output; +} + +bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose) +{ + try { + TransformStamped transform; + transform = tf_buffer.lookupTransform( + header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds(0.1)); + self_pose.position.x = transform.transform.translation.x; + self_pose.position.y = transform.transform.translation.y; + self_pose.position.z = transform.transform.translation.z; + self_pose.orientation.x = transform.transform.rotation.x; + self_pose.orientation.y = transform.transform.rotation.y; + self_pose.orientation.z = transform.transform.rotation.z; + self_pose.orientation.w = transform.transform.rotation.w; + return true; + } catch (tf2::TransformException & ex) { + return false; + } +} + +void getNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * nearest_collision_point, + rclcpp::Time * nearest_collision_point_time) +{ + double min_norm = 0.0; + bool is_init = false; + const auto yaw = getRPY(base_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + + for (const auto & p : pointcloud) { + const Eigen::Vector2d pointcloud_vec(p.x - base_pose.position.x, p.y - base_pose.position.y); + double norm = base_pose_vec.dot(pointcloud_vec); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = p; + *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; + is_init = true; + } + } +} + +void getLateralNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, + double * deviation) +{ + double min_norm = std::numeric_limits::max(); + const auto yaw = getRPY(base_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + for (size_t i = 0; i < pointcloud.size(); ++i) { + const Eigen::Vector2d pointcloud_vec( + pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y); + double norm = + std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); + if (norm < min_norm) { + min_norm = norm; + *lateral_nearest_point = pointcloud.at(i); + } + } + *deviation = min_norm; +} + +Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info) +{ + const auto & i = vehicle_info; + const auto yaw = getRPY(base_pose).z; + + Pose center_pose; + center_pose.position.x = + base_pose.position.x + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; +} +} // namespace motion_planning diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/CMakeLists.txt index f7bc92c..d6eba00 100644 --- a/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/CMakeLists.txt +++ b/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(autoware_auto_system_msgs REQUIRED) find_package(autoware_auto_vehicle_msgs REQUIRED) +find_package(autoware_auto_perception_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. @@ -19,7 +20,7 @@ find_package(autoware_auto_vehicle_msgs REQUIRED) add_executable(sample src/sample.cpp) -ament_target_dependencies(sample rclcpp std_msgs geometry_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs) +ament_target_dependencies(sample rclcpp std_msgs geometry_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_msgs) install(TARGETS diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/package.xml index 6514506..5fc5fbf 100644 --- a/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/package.xml +++ b/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/package.xml @@ -12,6 +12,7 @@ geometry_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_auto_perception_msgs rclcpp geometry_msgs std_msgs diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/src/sample.cpp b/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/src/sample.cpp index 9838fea..73944ac 100644 --- a/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/src/sample.cpp +++ b/autoware/aichallenge_ws/src/aichallenge_submit/sample_code_cpp/src/sample.cpp @@ -3,6 +3,8 @@ #include "autoware_auto_system_msgs/msg/autoware_state.hpp" #include "autoware_auto_vehicle_msgs/msg/engage.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "std_msgs/msg/string.hpp" #include @@ -19,10 +21,16 @@ class SampleCode : public rclcpp::Node this->create_publisher("/planning/mission_planning/goal", 1); check_pos_publisher = this->create_publisher( "/planning/mission_planning/checkpoint", 1); + initialpose_publisher = this->create_publisher( + "/initialpose", 1); + dummy_objects_publisher = this->create_publisher( + "/perception/object_recognition/objects", 1); // subscriber state_subscriber = this->create_subscription( "/autoware/state", 1, std::bind(&SampleCode::stateCallback, this, std::placeholders::_1)); + reset_pose_subscriber = this->create_subscription( + "/initialpose3d", 1, std::bind(&SampleCode::resetPoseCallback, this, std::placeholders::_1)); // msg check_info_msg = setUpMsgCheckInfo(); @@ -70,6 +78,17 @@ class SampleCode : public rclcpp::Node } } + + void resetPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped msg) + { + static bool is_active = true; + if (is_active) { + is_active = false; + initialpose_publisher->publish(setUpInitialpose()); + dummy_objects_publisher->publish(autoware_auto_perception_msgs::msg::PredictedObjects()); + } + } + // setup msg(engage) autoware_auto_vehicle_msgs::msg::Engage setUpMsgEngage() { @@ -93,6 +112,20 @@ class SampleCode : public rclcpp::Node return msg; } + geometry_msgs::msg::PoseWithCovarianceStamped setUpInitialpose() + { + auto msg = geometry_msgs::msg::PoseWithCovarianceStamped(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = 81377.429; + msg.pose.pose.position.y = 49917.175; + msg.pose.pose.position.z = 41.0; + msg.pose.pose.orientation.x = 0.0; + msg.pose.pose.orientation.y = 0.0; + msg.pose.pose.orientation.z = 0.2987379480292097; + msg.pose.pose.orientation.w = 0.9543330877776126; + return msg; + } + // setup msg(check info) geometry_msgs::msg::PoseStamped setUpMsgCheckInfo() @@ -113,9 +146,12 @@ class SampleCode : public rclcpp::Node rclcpp::Publisher::SharedPtr engage_publisher; rclcpp::Publisher::SharedPtr goal_pos_publisher; rclcpp::Publisher::SharedPtr check_pos_publisher; + rclcpp::Publisher::SharedPtr initialpose_publisher; + rclcpp::Publisher::SharedPtr dummy_objects_publisher; // subscriber rclcpp::Subscription::SharedPtr state_subscriber; + rclcpp::Subscription::SharedPtr reset_pose_subscriber; // msg geometry_msgs::msg::PoseStamped check_info_msg = geometry_msgs::msg::PoseStamped(); diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/CMakeLists.txt new file mode 100644 index 0000000..ce66dd3 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_autoware_api_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/README.md new file mode 100644 index 0000000..fe40916 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/README.md @@ -0,0 +1,21 @@ +# tier4_autoware_api_launch + +## Description + +This package contains launch files that run nodes to convert Autoware internal topics into consistent API used by external software (e.g., fleet management system, simulator). + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `autoware_api.launch.xml`. + +```xml + +``` + +## Notes + +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/autoware_api.launch.xml new file mode 100644 index 0000000..1feed03 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py new file mode 100644 index 0000000..fc730ff --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py @@ -0,0 +1,86 @@ +# Copyright 2021 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. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def _create_api_node(node_name, class_name, **kwargs): + return ComposableNode( + namespace="external", + name=node_name, + package="autoware_iv_external_api_adaptor", + plugin="external_api::" + class_name, + **kwargs + ) + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg("launch_calibration_status_api", None, "launch calibration status api") + add_launch_arg("launch_start_api", None, "launch start api") + + default_components = [ + _create_api_node("cpu_usage", "CpuUsage"), + _create_api_node("diagnostics", "Diagnostics"), + _create_api_node("door", "Door"), + _create_api_node("emergency", "Emergency"), + _create_api_node("engage", "Engage"), + _create_api_node("fail_safe_state", "FailSafeState"), + _create_api_node("initial_pose", "InitialPose"), + _create_api_node("map", "Map"), + _create_api_node("operator", "Operator"), + _create_api_node("metadata_packages", "MetadataPackages"), + _create_api_node("route", "Route"), + _create_api_node("rtc_controller", "RTCController"), + _create_api_node("service", "Service"), + _create_api_node("vehicle_status", "VehicleStatus"), + _create_api_node("velocity", "Velocity"), + _create_api_node("version", "Version"), + ] + container = ComposableNodeContainer( + namespace="external", + name="autoware_iv_adaptor", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=default_components, + output="screen", + ) + + calibration_status_loader = LoadComposableNodes( + composable_node_descriptions=[_create_api_node("calibration_status", "CalibrationStatus")], + target_container=container, + condition=IfCondition(LaunchConfiguration("launch_calibration_status_api")), + ) + + start_loader = LoadComposableNodes( + composable_node_descriptions=[_create_api_node("start", "Start")], + target_container=container, + condition=IfCondition(LaunchConfiguration("launch_start_api")), + ) + + return launch.LaunchDescription( + launch_arguments + [container, calibration_status_loader, start_loader] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py new file mode 100644 index 0000000..422c35a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py @@ -0,0 +1,45 @@ +# Copyright 2021 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. + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _create_api_node(node_name, class_name, **kwargs): + return ComposableNode( + namespace="internal", + name=node_name, + package="autoware_iv_internal_api_adaptor", + plugin="internal_api::" + class_name, + **kwargs + ) + + +def generate_launch_description(): + components = [ + _create_api_node("iv_msgs", "IVMsgs"), + _create_api_node("operator", "Operator"), + _create_api_node("route", "Route"), + _create_api_node("velocity", "Velocity"), + ] + container = ComposableNodeContainer( + namespace="internal", + name="autoware_iv_adaptor", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=components, + output="screen", + ) + return launch.LaunchDescription([container]) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml new file mode 100644 index 0000000..27c6252 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/package.xml new file mode 100644 index 0000000..44a78ff --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_autoware_api_launch/package.xml @@ -0,0 +1,29 @@ + + + + tier4_autoware_api_launch + 0.0.0 + The tier4_autoware_api_launch package + Takagi, Isamu + Ryohsuke Mitsudome + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + ad_api_adaptors + autoware_iv_external_api_adaptor + autoware_iv_internal_api_adaptor + awapi_awiv_adapter + default_ad_api + path_distance_calculator + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/CMakeLists.txt new file mode 100644 index 0000000..0686243 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_control_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/README.md new file mode 100644 index 0000000..919258c --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/README.md @@ -0,0 +1,24 @@ +# tier4_control_launch + +## Structure + +![tier4_control_launch](./control_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `control.launch.py`. + +```xml + + + + +``` + +## Notes + +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/common/nearest_search.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/common/nearest_search.param.yaml new file mode 100644 index 0000000..eb67097 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/common/nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml new file mode 100644 index 0000000..d42e239 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + frequency_hz: 10.0 + engage_acceptable_limits: + allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. + dist_threshold: 1.5 + yaw_threshold: 0.524 + speed_upper_threshold: 10.0 + speed_lower_threshold: -10.0 + acc_threshold: 1.5 + lateral_acc_threshold: 1.0 + lateral_acc_diff_threshold: 0.5 + stable_check: + duration: 0.1 + dist_threshold: 1.5 + speed_upper_threshold: 2.0 + speed_lower_threshold: -2.0 + yaw_threshold: 0.262 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/shift_decider/shift_decider.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/shift_decider/shift_decider.param.yaml new file mode 100644 index 0000000..4c45b36 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/shift_decider/shift_decider.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + park_on_goal: true diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml new file mode 100644 index 0000000..76b5da1 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -0,0 +1,74 @@ +/**: + ros__parameters: + + # -- system -- + traj_resample_dist: 0.1 # path resampling interval [m] + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) + admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + + # -- path smoothing -- + enable_path_smoothing: false # flag for path smoothing + path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing + curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + + # -- mpc optimization -- + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q + mpc_weight_steering_input: 1.0 # steering error weight in matrix R + mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R + mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R + mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability + mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability + mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero + mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration + mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length + + # -- vehicle model -- + vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics + input_delay: 0.24 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_rate_lim_dps: 40.0 # steering angle rate limit [deg/s] + acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] + + # -- lowpass filter for noise reduction -- + steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + error_deriv_lpf_cutoff_hz: 5.0 + + # stop state: steering command is kept in the previous value in the stop state. + stop_state_entry_ego_speed: 0.001 + stop_state_entry_target_speed: 0.001 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: true + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 + + # vehicle parameters + vehicle: + cg_to_front_m: 1.228 + cg_to_rear_m: 1.5618 + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml new file mode 100644 index 0000000..b6e1c3a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -0,0 +1,86 @@ +/**: + ros__parameters: + delay_compensation_time: 0.17 + + enable_smooth_stop: true + enable_overshoot_emergency: true + enable_large_tracking_error_emergency: true + enable_slope_compensation: false + enable_keep_stopped_until_steer_convergence: true + + # state transition + drive_state_stop_dist: 0.5 + drive_state_offset_stop_dist: 1.0 + stopping_state_stop_dist: 0.49 + stopped_state_entry_duration_time: 0.1 + stopped_state_entry_vel: 0.1 + stopped_state_entry_acc: 0.1 + emergency_state_overshoot_stop_dist: 1.5 + emergency_state_traj_trans_dev: 3.0 + emergency_state_traj_rot_dev: 0.7 + + # drive state + kp: 1.0 + ki: 0.1 + kd: 0.0 + max_out: 1.0 + min_out: -1.0 + max_p_effort: 1.0 + min_p_effort: -1.0 + max_i_effort: 0.3 + min_i_effort: -0.3 + max_d_effort: 0.0 + min_d_effort: 0.0 + lpf_vel_error_gain: 0.9 + current_vel_threshold_pid_integration: 0.5 + enable_brake_keeping_before_stop: false + brake_keeping_acc: -0.2 + + # smooth stop state + smooth_stop_max_strong_acc: -0.5 + smooth_stop_min_strong_acc: -1.0 + smooth_stop_weak_acc: -0.3 + smooth_stop_weak_stop_acc: -0.8 + smooth_stop_strong_stop_acc: -3.4 + smooth_stop_max_fast_vel: 0.5 + smooth_stop_min_running_vel: 0.01 + smooth_stop_min_running_acc: 0.01 + smooth_stop_weak_stop_time: 0.8 + smooth_stop_weak_stop_dist: -0.3 + smooth_stop_strong_stop_dist: -0.5 + + # stopped state + stopped_vel: 0.0 + stopped_acc: -3.4 + stopped_jerk: -5.0 + + # emergency state + emergency_vel: 0.0 + emergency_acc: -5.0 + emergency_jerk: -3.0 + + # acceleration limit + max_acc: 3.0 + min_acc: -5.0 + + # jerk limit + max_jerk: 2.0 + min_jerk: -5.0 + + # pitch + use_trajectory_for_pitch_calculation: false + lpf_pitch_gain: 0.95 + max_pitch_rad: 0.1 + min_pitch_rad: -0.1 + + # vehicle parameters + vehicle: + cg_to_front_m: 1.228 + cg_to_rear_m: 1.5618 + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml new file mode 100644 index 0000000..4876699 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + update_rate: 10.0 + system_emergency_heartbeat_timeout: 0.5 + external_emergency_stop_heartbeat_timeout: 0.0 + stop_hold_acceleration: -1.5 + emergency_acceleration: -2.4 + stopped_state_entry_duration_time: 0.1 + nominal: + vel_lim: 25.0 + lon_acc_lim: 5.0 + lon_jerk_lim: 5.0 + lat_acc_lim: 5.0 + lat_jerk_lim: 5.0 + on_transition: + vel_lim: 50.0 + lon_acc_lim: 1.0 + lon_jerk_lim: 0.5 + lat_acc_lim: 1.2 + lat_jerk_lim: 0.75 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/control_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/control_launch.drawio.svg new file mode 100644 index 0000000..0f5b4fe --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/control_launch.drawio.svg @@ -0,0 +1,421 @@ + + + + + + + +
+
+
+ container +
+
+
+ + package: rclcpp_components + +
+
+
+
+
+ + container... + +
+
+ + + + +
+
+
+ control.launch.py +
+
+
+ + package: tier4_control_launch + +
+
+
+
+
+ + control.launch.py... + +
+
+ + + + + + + +
+
+
+ mpc_follower +
+
+
+
+ + mpc_follower + +
+
+ + + + + +
+
+
+ pure_pursuit +
+
+
+
+ + pure_pursuit + +
+
+ + + + +
+
+
+ $(var lateral_controller_mode) +
+
+
+
+ + $(var lateral_controller_mode) + +
+
+ + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+
+ + launch name... + +
+
+ + + + +
+
+
+ ex: +
+
+
+
+ + ex: + +
+
+ + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+
+ + node name... + +
+
+ + + + +
+
+
+ latlon_muxer +
+
+
+ + package: latlon_muxer + +
+
+
+
+
+ + latlon_muxer... + +
+
+ + + + +
+
+
+ mpc_follower +
+
+
+ + package: mpc_follower + +
+
+
+
+
+ + mpc_follower... + +
+
+ + + + +
+
+
+ pure_pursuit +
+
+
+ + package: pure_pursuit + +
+
+
+
+
+ + pure_pursuit... + +
+
+ + + + +
+
+
+ velocity_controller +
+
+
+ + package: velocity_controller + +
+
+
+
+
+ + velocity_controller... + +
+
+ + + + +
+
+
+ lane_departure_checker_node +
+
+
+ + package: lane_departure_checker_node + +
+
+
+
+
+ + lane_departure_checker_node... + +
+
+ + + + +
+
+
+ shift_decider +
+
+
+ + package: shift_decider + +
+
+
+
+
+ + shift_decider... + +
+
+ + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + package: vehicle_cmd_gate + +
+
+
+
+
+ + vehicle_cmd_gate... + +
+
+ + + + +
+
+
+ external_cmd_converter +
+
+
+ + package: external_cmd_converter + +
+
+
+
+
+ + external_cmd_converter... + +
+
+ + + + +
+
+
+ external_cmd_selector +
+
+
+ + package: external_cmd_selector + +
+
+
+
+
+ + external_cmd_selector... + +
+
+ + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+
+ + other name... + +
+
+
+ + + + + Viewer does not support full SVG 1.1 + + + +
diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/launch/control.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/launch/control.launch.py new file mode 100644 index 0000000..d9ed65f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/launch/control.launch.py @@ -0,0 +1,355 @@ +# 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. + +import os + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + lat_controller_param_path = os.path.join( + LaunchConfiguration("tier4_control_launch_param_path").perform(context), + "trajectory_follower", + "lateral_controller.param.yaml", + ) + with open(lat_controller_param_path, "r") as f: + lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + nearest_search_param_path = os.path.join( + LaunchConfiguration("tier4_control_launch_param_path").perform(context), + "common", + "nearest_search.param.yaml", + ) + with open(nearest_search_param_path, "r") as f: + nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + lon_controller_param_path = os.path.join( + LaunchConfiguration("tier4_control_launch_param_path").perform(context), + "trajectory_follower", + "longitudinal_controller.param.yaml", + ) + with open(lon_controller_param_path, "r") as f: + lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + vehicle_cmd_gate_param_path = os.path.join( + LaunchConfiguration("tier4_control_launch_param_path").perform(context), + "vehicle_cmd_gate", + "vehicle_cmd_gate.param.yaml", + ) + with open(vehicle_cmd_gate_param_path, "r") as f: + vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + lane_departure_checker_param_path = LaunchConfiguration( + "lane_departure_checker_param_path" + ).perform(context) + with open(lane_departure_checker_param_path, "r") as f: + lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + operation_mode_transition_manager_param_path = os.path.join( + LaunchConfiguration("tier4_control_launch_param_path").perform(context), + "operation_mode_transition_manager", + "operation_mode_transition_manager.param.yaml", + ) + with open(operation_mode_transition_manager_param_path, "r") as f: + operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + shift_decider_param_path = os.path.join( + LaunchConfiguration("tier4_control_launch_param_path").perform(context), + "shift_decider", + "shift_decider.param.yaml", + ) + with open(shift_decider_param_path, "r") as f: + shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + controller_component = ComposableNode( + package="trajectory_follower_nodes", + plugin="autoware::motion::control::trajectory_follower_nodes::Controller", + name="controller_node_exe", + namespace="trajectory_follower", + remappings=[ + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("~/input/current_odometry", "/localization/kinematic_state"), + ("~/input/current_steering", "/vehicle/status/steering_status"), + ("~/input/current_accel", "/localization/acceleration"), + ("~/output/predicted_trajectory", "lateral/predicted_trajectory"), + ("~/output/lateral_diagnostic", "lateral/diagnostic"), + ("~/output/slope_angle", "longitudinal/slope_angle"), + ("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"), + ("~/output/control_cmd", "control_cmd"), + ], + parameters=[ + { + "ctrl_period": 0.03, + "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), + }, + nearest_search_param, + lon_controller_param, + lat_controller_param, + vehicle_info_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # lane departure checker + lane_departure_component = ComposableNode( + package="lane_departure_checker", + plugin="lane_departure_checker::LaneDepartureCheckerNode", + name="lane_departure_checker_node", + namespace="trajectory_follower", + remappings=[ + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/lanelet_map_bin", "/map/vector_map"), + ("~/input/route", "/planning/mission_planning/route"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[nearest_search_param, lane_departure_checker_param, vehicle_info_param], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # shift decider + shift_decider_component = ComposableNode( + package="shift_decider", + plugin="ShiftDecider", + name="shift_decider", + remappings=[ + ("input/control_cmd", "/control/trajectory_follower/control_cmd"), + ("input/state", "/autoware/state"), + ("output/gear_cmd", "/control/shift_decider/gear_cmd"), + ], + parameters=[ + shift_decider_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # vehicle cmd gate + vehicle_cmd_gate_component = ComposableNode( + package="vehicle_cmd_gate", + plugin="vehicle_cmd_gate::VehicleCmdGate", + name="vehicle_cmd_gate", + remappings=[ + ("input/emergency_state", "/system/emergency/emergency_state"), + ("input/steering", "/vehicle/status/steering_status"), + ("input/operation_mode", "/control/operation_mode"), + ("input/auto/control_cmd", "/control/trajectory_follower/control_cmd"), + ("input/auto/turn_indicators_cmd", "/planning/turn_indicators_cmd"), + ("input/auto/hazard_lights_cmd", "/planning/hazard_lights_cmd"), + ("input/auto/gear_cmd", "/control/shift_decider/gear_cmd"), + ("input/external/control_cmd", "/external/selected/control_cmd"), + ("input/external/turn_indicators_cmd", "/external/selected/turn_indicators_cmd"), + ("input/external/hazard_lights_cmd", "/external/selected/hazard_lights_cmd"), + ("input/external/gear_cmd", "/external/selected/gear_cmd"), + ("input/external_emergency_stop_heartbeat", "/external/selected/heartbeat"), + ("input/gate_mode", "/control/gate_mode_cmd"), + ("input/emergency/control_cmd", "/system/emergency/control_cmd"), + ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"), + ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"), + ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"), + ("output/control_cmd", "/control/command/control_cmd"), + ("output/gear_cmd", "/control/command/gear_cmd"), + ("output/turn_indicators_cmd", "/control/command/turn_indicators_cmd"), + ("output/hazard_lights_cmd", "/control/command/hazard_lights_cmd"), + ("output/gate_mode", "/control/current_gate_mode"), + ("output/engage", "/api/autoware/get/engage"), + ("output/external_emergency", "/api/autoware/get/emergency"), + ("output/operation_mode", "/control/vehicle_cmd_gate/operation_mode"), + ("~/service/engage", "/api/autoware/set/engage"), + ("~/service/external_emergency", "/api/autoware/set/emergency"), + # TODO(Takagi, Isamu): deprecated + ("input/engage", "/autoware/engage"), + ("~/service/external_emergency_stop", "~/external_emergency_stop"), + ("~/service/clear_external_emergency_stop", "~/clear_external_emergency_stop"), + ], + parameters=[ + vehicle_cmd_gate_param, + vehicle_info_param, + { + "use_emergency_handling": LaunchConfiguration("use_emergency_handling"), + "use_external_emergency_stop": LaunchConfiguration("use_external_emergency_stop"), + "use_start_request": LaunchConfiguration("use_start_request"), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # operation mode transition manager + operation_mode_transition_manager_component = ComposableNode( + package="operation_mode_transition_manager", + plugin="operation_mode_transition_manager::OperationModeTransitionManager", + name="operation_mode_transition_manager", + remappings=[ + # input + ("kinematics", "/localization/kinematic_state"), + ("steering", "/vehicle/status/steering_status"), + ("trajectory", "/planning/scenario_planning/trajectory"), + ("control_cmd", "/control/command/control_cmd"), + ("control_mode_report", "/vehicle/status/control_mode"), + ("gate_operation_mode", "/control/vehicle_cmd_gate/operation_mode"), + ("operation_mode_request", "/system/operation_mode_request"), + # output + ("is_autonomous_available", "/control/is_autonomous_available"), + ("operation_mode", "/control/operation_mode"), + ("control_mode_request", "/control/control_mode_request"), + ], + parameters=[ + nearest_search_param_path, + operation_mode_transition_manager_param, + vehicle_info_param, + ], + ) + + # external cmd selector + external_cmd_selector_loader = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("external_cmd_selector"), "/launch/external_cmd_selector.launch.py"] + ), + launch_arguments=[ + ("use_intra_process", LaunchConfiguration("use_intra_process")), + ("target_container", "/control/control_container"), + ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + ], + ) + + # external cmd converter + external_cmd_converter_loader = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("external_cmd_converter"), "/launch/external_cmd_converter.launch.py"] + ), + launch_arguments=[ + ("use_intra_process", LaunchConfiguration("use_intra_process")), + ("target_container", "/control/control_container"), + ], + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name="control_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + controller_component, + lane_departure_component, + shift_decider_component, + vehicle_cmd_gate_component, + operation_mode_transition_manager_component, + ], + ) + + group = GroupAction( + [ + PushRosNamespace("control"), + container, + external_cmd_selector_loader, + external_cmd_converter_loader, + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + # parameter + add_launch_arg( + "tier4_control_launch_param_path", + [ + FindPackageShare("tier4_control_launch"), + "/config", + ], + "tier4_control_launch parameter path", + ) + + # lateral controller + add_launch_arg( + "lateral_controller_mode", + "mpc_follower", + "lateral controller mode: `mpc_follower` or `pure_pursuit`", + ) + + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + add_launch_arg( + "lane_departure_checker_param_path", + [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"], + ) + + # velocity controller + add_launch_arg("show_debug_info", "false", "show debug information") + add_launch_arg("enable_pub_debug", "true", "enable to publish debug information") + + # vehicle cmd gate + add_launch_arg("use_emergency_handling", "false", "use emergency handling") + add_launch_arg("use_external_emergency_stop", "true", "use external emergency stop") + add_launch_arg("use_start_request", "false", "use start request service") + + # external cmd selector + add_launch_arg("initial_selector_mode", "remote", "local or remote") + + # component + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/package.xml new file mode 100644 index 0000000..8d9b3d9 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_control_launch/package.xml @@ -0,0 +1,27 @@ + + + + tier4_control_launch + 0.1.0 + The tier4_control_launch package + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + external_cmd_converter + lane_departure_checker + shift_decider + trajectory_follower + trajectory_follower_nodes + vehicle_cmd_gate + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/CMakeLists.txt new file mode 100644 index 0000000..056fe87 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_localization_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/README.md new file mode 100644 index 0000000..ee7d6ca --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/README.md @@ -0,0 +1,26 @@ +# tier4_localization_launch + +## Structure + +![tier4_localization_launch](./localization_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `localization.launch.xml`. + +```xml + + +``` + +## Notes + +There are some `param.yaml` files in `config` directory. + +```bash +ndt_scan_matcher.param.yaml +``` diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml new file mode 100644 index 0000000..ad55423 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + input_frame: "base_link" + output_frame: "base_link" + min_x: -60.0 + max_x: 60.0 + min_y: -60.0 + max_y: 60.0 + min_z: -30.0 + max_z: 50.0 + negative: False diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/localization_error_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/localization_error_monitor.param.yaml new file mode 100644 index 0000000..026daf0 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/localization_error_monitor.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + scale: 3.0 + error_ellipse_size: 1.0 + warn_ellipse_size: 0.8 + error_ellipse_size_lateral_direction: 0.3 + warn_ellipse_size_lateral_direction: 0.2 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/ndt_scan_matcher.param.yaml new file mode 100644 index 0000000..05b6d4e --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/ndt_scan_matcher.param.yaml @@ -0,0 +1,71 @@ +/**: + ros__parameters: + # Vehicle reference frame + base_frame: "base_link" + + # Subscriber queue size + input_sensor_points_queue_size: 1 + + # NDT implementation type + # 0=PCL_GENERIC, 1=PCL_MODIFIED, 2=OMP + ndt_implement_type: 2 + + # The maximum difference between two consecutive + # transformations in order to consider convergence + trans_epsilon: 0.01 + + # The newton line search maximum step length + step_size: 0.1 + + # The ND voxel grid resolution + resolution: 2.0 + + # The number of iterations required to calculate alignment + max_iterations: 30 + + # Converged param type + # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD + # NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD is only available when NDTImplementType::OMP is selected + converged_param_type: 1 + + # If converged_param_type is 0 + # Threshold for deciding whether to trust the estimation result + converged_param_transform_probability: 3.0 + + # If converged_param_type is 1 + # Threshold for deciding whether to trust the estimation result + converged_param_nearest_voxel_transformation_likelihood: 2.3 + + # The number of particles to estimate initial pose + initial_estimate_particles_num: 100 + + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 + + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 + + # neighborhood search method in OMP + # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 + omp_neighborhood_search_method: 0 + + # Number of threads used for parallel computing + omp_num_threads: 4 + + # The covariance of output pose + # Do note that this covariance matrix is empirically derived + output_pose_covariance: + [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, + ] + + # Regularization switch + regularization_enabled: false + + # regularization scale factor + regularization_scale_factor: 0.01 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/random_downsample_filter.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/random_downsample_filter.param.yaml new file mode 100644 index 0000000..53be849 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/random_downsample_filter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + sample_num: 1500 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/voxel_grid_filter.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/voxel_grid_filter.param.yaml new file mode 100644 index 0000000..51a7ee9 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/config/voxel_grid_filter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + voxel_size_x: 3.0 + voxel_size_y: 3.0 + voxel_size_z: 3.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/localization.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/localization.launch.xml new file mode 100644 index 0000000..1b1e43f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/localization.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml new file mode 100644 index 0000000..43cfc74 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml new file mode 100644 index 0000000..750adca --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml new file mode 100644 index 0000000..3c10976 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml new file mode 100644 index 0000000..3627c66 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/util/util.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/util/util.launch.py new file mode 100644 index 0000000..24da889 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/util/util.launch.py @@ -0,0 +1,146 @@ +# 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. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.conditions import LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + # https://github.com/ros2/launch_ros/issues/156 + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + crop_box_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_measurement_range", + remappings=[ + ("input", LaunchConfiguration("input/pointcloud")), + ("output", "measurement_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("crop_box_filter_measurement_range_param_path"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + voxel_grid_downsample_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + name="voxel_grid_downsample_filter", + remappings=[ + ("input", "measurement_range/pointcloud"), + ("output", "voxel_grid_downsample/pointcloud"), + ], + parameters=[load_composable_node_param("voxel_grid_downsample_filter_param_path")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + random_downsample_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent", + name="random_downsample_filter", + remappings=[ + ("input", "voxel_grid_downsample/pointcloud"), + ("output", LaunchConfiguration("output/pointcloud")), + ], + parameters=[load_composable_node_param("random_downsample_filter_param_path")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + composable_nodes = [ + crop_box_component, + voxel_grid_downsample_component, + random_downsample_component, + ] + + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals("container", ""), + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container"), + ) + + return [load_composable_nodes] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + arg = DeclareLaunchArgument(name, default_value=default_value, description=description) + launch_arguments.append(arg) + + add_launch_arg( + "tier4_localization_launch_param_path", + [FindPackageShare("tier4_localization_launch"), "/config"], + "tier4_localization_launch param path", + ) + add_launch_arg( + "crop_box_filter_measurement_range_param_path", + [ + LaunchConfiguration("tier4_localization_launch_param_path"), + "/crop_box_filter_measurement_range.param.yaml", + ], + "path to the parameter file of crop_box_filter_measurement_range", + ) + add_launch_arg( + "voxel_grid_downsample_filter_param_path", + [ + LaunchConfiguration("tier4_localization_launch_param_path"), + "/voxel_grid_filter.param.yaml", + ], + "path to the parameter file of voxel_grid_downsample_filter", + ) + add_launch_arg( + "random_downsample_filter_param_path", + [ + LaunchConfiguration("tier4_localization_launch_param_path"), + "/random_downsample_filter.param.yaml", + ], + "path to the parameter file of random_downsample_filter", + ) + add_launch_arg("use_intra_process", "true", "use ROS2 component container communication") + add_launch_arg( + "container", + "/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container", + "container name", + ) + add_launch_arg( + "output/pointcloud", + "downsample/pointcloud", + "final output topic name", + ) + add_launch_arg( + "output_measurement_range_sensor_points_topic", + "measurement_range/pointcloud", + "output topic name for crop box filter", + ) + add_launch_arg( + "output_voxel_grid_downsample_sensor_points_topic", + "voxel_grid_downsample/pointcloud", + "output topic name for voxel grid downsample filter", + ) + add_launch_arg( + "output_downsample_sensor_points_topic", + "downsample/pointcloud", + "output topic name for downsample filter. this is final output", + ) + + return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/util/util.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/util/util.launch.xml new file mode 100644 index 0000000..e0fdacd --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/launch/util/util.launch.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/localization_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/localization_launch.drawio.svg new file mode 100644 index 0000000..3dc8766 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/localization_launch.drawio.svg @@ -0,0 +1,530 @@ + + + + + + + +
+
+
+ /sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container +
+
+
+ + package: rclcpp_components + +
+
+
+
+
+ + /sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container... + +
+
+ + + + +
+
+
+ localization.launch.xml +
+
+
+ + package: localization + + + _launch + +
+
+
+
+
+ + localization.launch.xml... + +
+
+ + + + + + + + +
+
+
+ util.launch.xml +
+
+
+ package: tier4_localization_launch +
+
+
+
+
+ + util.launch.xml... + +
+
+ + + + + + + + +
+
+
+ pose_estimator.launch.xml +
+
+
+ + package: tier4_localization_launch + +
+
+
+
+
+ + pose_estimator.launch.xml... + +
+
+ + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+
+ + launch name... + +
+
+ + + + +
+
+
+ ex: +
+
+
+
+ + ex: + +
+
+ + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+
+ + node name... + +
+
+ + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+
+ + other name... + +
+
+ + + + + + +
+
+
+ twist_estimator.launch.xml +
+
+
+ + package: tier4_localization_launch + +
+
+
+
+
+ + twist_estimator.launch.xml... + +
+
+ + + + + + +
+
+
+ pose_twist_fusion_filter.launch.xml +
+
+
+ + package: tier4_localization_launch + +
+
+
+
+
+ + pose_twist_fusion_filter.launch.xml... + +
+
+ + + + + + +
+
+
+ localization_error_monitor.launch.xml +
+
+
+ + package: localization_error_monitor + +
+
+
+
+
+ + localization_error_monitor.launch.xml... + +
+
+ + + + + + +
+
+
+ relay +
+
+
+ + package: topic_tools + +
+
+ +
+
+
+
+ + input: /localization/pose_twist_fusion_filter/twist + +
+
+ + output: /localization/twist + +
+
+
+
+
+ + relay... + +
+
+ + + + + + +
+
+
+ ndt_scan_matcher.launch.xml +
+
+
+ + package: ndt_scan_matcher + +
+
+
+
+
+ + ndt_scan_matcher.launch.xml... + +
+
+ + + + + + +
+
+
+ gyro_odometer.launch.xml +
+
+
+ + package: gyro_odometer + +
+
+
+
+
+ + gyro_odometer.launch.xml... + +
+
+ + + + + + +
+
+
+ ekf_localizer.launch.xml +
+
+
+ + package: ekf_localizer + +
+
+
+
+
+ + ekf_localizer.launch.xml... + +
+
+ + + + + + +
+
+
+ pose_initializer.launch.xml +
+
+
+ + package: pose_initializer + +
+
+
+
+
+ + pose_initializer.launch.xml... + +
+
+ + + + +
+
+
+ util.launch.py +
+
+
+ + package: tier4_localization_launch + +
+
+
+
+
+ + util.launch.py... + +
+
+ + + + + + +
+
+
+ crop_box_measurement_range +
+
+
+ + package: pointcloud_preprocessor + +
+
+
+
+
+ + crop_box_measurement_range... + +
+
+ + + + +
+
+
+ voxel_grid_downsample_filter +
+
+
+ + package: pointcloud_preprocessor + +
+
+
+
+
+ + voxel_grid_downsample_filter... + +
+
+ + + + +
+
+
+ random_downsample_filter +
+
+
+ + package: pointcloud_preprocessor + +
+
+
+
+
+ + random_downsample_filter... + +
+
+ + +
+ + + + + Viewer does not support full SVG 1.1 + + + +
diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/package.xml new file mode 100644 index 0000000..13102ea --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_localization_launch/package.xml @@ -0,0 +1,29 @@ + + + + tier4_localization_launch + 0.1.0 + The tier4_localization_launch package + Yamato Ando + Apache License 2.0 + Yamato Ando + + ament_cmake_auto + + autoware_cmake + + automatic_pose_initializer + ekf_localizer + gyro_odometer + ndt_scan_matcher + pointcloud_preprocessor + pose_initializer + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/CMakeLists.txt new file mode 100644 index 0000000..98f12b6 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_map_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/README.md new file mode 100644 index 0000000..344a56a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/README.md @@ -0,0 +1,28 @@ +# tier4_map_launch + +## Structure + +![tier4_map_launch](./map_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `map.launch.py`. + +```xml + + + + + + + + +``` + +## Notes + +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/launch/map.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/launch/map.launch.py new file mode 100644 index 0000000..6d5265f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/launch/map.launch.py @@ -0,0 +1,174 @@ +# Copyright 2021 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. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( + context + ) + + with open(lanelet2_map_loader_param_path, "r") as f: + lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + map_hash_generator = Node( + package="map_loader", + executable="map_hash_generator", + name="map_hash_generator", + parameters=[ + { + "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), + "pointcloud_map_path": LaunchConfiguration("pointcloud_map_path"), + } + ], + ) + + lanelet2_map_loader = ComposableNode( + package="map_loader", + plugin="Lanelet2MapLoaderNode", + name="lanelet2_map_loader", + remappings=[("output/lanelet2_map", "vector_map")], + parameters=[ + { + "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), + }, + lanelet2_map_loader_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + lanelet2_map_visualization = ComposableNode( + package="map_loader", + plugin="Lanelet2MapVisualizationNode", + name="lanelet2_map_visualization", + remappings=[ + ("input/lanelet2_map", "vector_map"), + ("output/lanelet2_map_marker", "vector_map_marker"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + pointcloud_map_loader = ComposableNode( + package="map_loader", + plugin="PointCloudMapLoaderNode", + name="pointcloud_map_loader", + remappings=[("output/pointcloud_map", "pointcloud_map")], + parameters=[ + {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]} + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + map_tf_generator = ComposableNode( + package="map_tf_generator", + plugin="VectorMapTFGeneratorNode", + name="vector_map_tf_generator", + parameters=[ + { + "map_frame": "map", + "viewer_frame": "viewer", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="map_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + lanelet2_map_loader, + lanelet2_map_visualization, + pointcloud_map_loader, + map_tf_generator, + ], + output="screen", + ) + + group = GroupAction( + [ + PushRosNamespace("map"), + container, + map_hash_generator, + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg("map_path", "", "path to map directory"), + add_launch_arg( + "lanelet2_map_path", + [LaunchConfiguration("map_path"), "/lanelet2_map.osm"], + "path to lanelet2 map file", + ), + add_launch_arg( + "pointcloud_map_path", + [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"], + "path to pointcloud map file", + ), + add_launch_arg( + "lanelet2_map_loader_param_path", + [ + FindPackageShare("map_loader"), + "/config/lanelet2_map_loader.param.yaml", + ], + "path to lanelet2_map_loader param file", + ), + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"), + add_launch_arg("use_multithread", "false", "use multithread"), + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/launch/map.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/launch/map.launch.xml new file mode 100644 index 0000000..7dbdd21 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/launch/map.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/map_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/map_launch.drawio.svg new file mode 100644 index 0000000..cabae39 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/map_launch.drawio.svg @@ -0,0 +1,250 @@ + + + + + + + +
+
+
+ map_container +
+
+
+ + package: rclcpp_components + +
+
+
+
+
+ + map_container... + +
+
+ + + + +
+
+
+ map.launch.py +
+
+
+ + package: tier4_map_launch + +
+
+
+
+
+ + map.launch.py... + +
+
+ + + + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+
+ + launch name... + +
+
+ + + + +
+
+
+ ex: +
+
+
+
+ + ex: + +
+
+ + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+
+ + node name... + +
+
+ + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+
+ + other name... + +
+
+ + + + +
+
+
+ lanelet2_map_loader +
+
+
+ + package: map_loader + +
+
+
+
+
+ + lanelet2_map_loader... + +
+
+ + + + +
+
+
+ lanelet2_map_visualization +
+
+
+ + package: map_loader + +
+
+
+
+
+ + lanelet2_map_visualization... + +
+
+ + + + +
+
+
+ pointcloud_map_loader +
+
+
+ + package: map_loader + +
+
+
+
+
+ + pointcloud_map_loader... + +
+
+ + + + +
+
+
+ + map_tf_generator + +
+
+
+ + package: map_tf_generator + +
+
+
+
+
+ + map_tf_generator... + +
+
+
+ + + + + Viewer does not support full SVG 1.1 + + + +
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/package.xml new file mode 100644 index 0000000..683d7b7 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_map_launch/package.xml @@ -0,0 +1,23 @@ + + + + tier4_map_launch + 0.1.0 + The tier4_map_launch package + Ryohsuke Mitsudome + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + map_loader + map_tf_generator + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/CMakeLists.txt new file mode 100644 index 0000000..4962a29 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_perception_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/README.md new file mode 100644 index 0000000..a81c435 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/README.md @@ -0,0 +1,20 @@ +# tier4_perception_launch + +## Structure + +![tier4_perception_launch](./perception_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `perception.launch.xml`. + +```xml + + + + +``` diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml new file mode 100644 index 0000000..dfdee95 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + filter_target_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml new file mode 100644 index 0000000..70afd9d --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + filter_target_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false + + upper_bound_x: 100.0 + lower_bound_x: 0.0 + upper_bound_y: 10.0 + lower_bound_y: -10.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml new file mode 100644 index 0000000..a07a941 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + use_down_sample_filter: False + down_sample_voxel_size: 0.1 + distance_threshold: 0.5 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml new file mode 100644 index 0000000..1790315 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -0,0 +1,66 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + max_area_matrix: + # NOTE(yukke42): The size of truck is 12 m length x 3 m width. + # NOTE(yukke42): The size of trailer is 20 m length x 3 m width. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN + 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR + 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR + 6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml new file mode 100644 index 0000000..781ccb8 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml @@ -0,0 +1,30 @@ +pcl_grid_map_extraction: + num_processing_threads: 12 + cloud_transform: + translation: + x: 0.0 + y: 0.0 + z: 0.0 + rotation: # intrinsic rotation X-Y-Z (r-p-y)sequence + r: 0.0 + p: 0.0 + y: 0.0 + cluster_extraction: + cluster_tolerance: 0.2 + min_num_points: 3 + max_num_points: 1000000 + outlier_removal: + is_remove_outliers: false + mean_K: 10 + stddev_threshold: 1.0 + downsampling: + is_downsample_cloud: false + voxel_size: + x: 0.02 + y: 0.02 + z: 0.02 + grid_map: + min_num_points_per_cell: 3 + resolution: 0.3 + height_type: 1 + height_thresh: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml new file mode 100644 index 0000000..bde1b02 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + additional_lidars: [] + ransac_input_topics: [] + use_single_frame_filter: False + use_time_series_filter: True + + common_crop_box_filter: + parameters: + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 + max_z: 10.7 # recommended 2.5 for non elevation_grid_mode + min_z: -8.7 # recommended 0.0 for non elevation_grid_mode + negative: False + + common_ground_filter: + plugin: "ground_segmentation::ScanGroundFilterComponent" + parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.2 + use_virtual_ground_point: True + split_height_distance: 0.2 + non_ground_height_threshold: 0.20 + grid_size_m: 0.1 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 + elevation_grid_mode: true diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml new file mode 100644 index 0000000..9778aaf --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -0,0 +1,245 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml new file mode 100644 index 0000000..e373d45 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml new file mode 100644 index 0000000..cf42be9 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml new file mode 100644 index 0000000..5746033 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml new file mode 100644 index 0000000..02e143f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py new file mode 100644 index 0000000..f6a88bb --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -0,0 +1,168 @@ +# Copyright 2022 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. +import os + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import yaml + + +class PointcloudMapFilterPipeline: + def __init__(self, context): + pointcloud_map_filter_param_path = os.path.join( + LaunchConfiguration("tier4_perception_launch_param_path").perform(context), + "object_recognition/detection/pointcloud_map_filter.param.yaml", + ) + with open(pointcloud_map_filter_param_path, "r") as f: + self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] + self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] + self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] + self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] + + def create_pipeline(self): + if self.use_down_sample_filter: + return self.create_down_sample_pipeline() + else: + return self.create_normal_pipeline() + + def create_normal_pipeline(self): + components = [] + components.append( + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", + name="voxel_based_compare_map_filter", + remappings=[ + ("input", LaunchConfiguration("input_topic")), + ("map", "/map/pointcloud_map"), + ("output", LaunchConfiguration("output_topic")), + ], + parameters=[ + { + "distance_threshold": self.distance_threshold, + } + ], + extra_arguments=[ + {"use_intra_process_comms": False}, + ], + ) + ) + return components + + def create_down_sample_pipeline(self): + components = [] + down_sample_topic = ( + "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" + ) + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + name="voxel_grid_downsample_filter", + remappings=[ + ("input", LaunchConfiguration("input_topic")), + ("output", down_sample_topic), + ], + parameters=[ + { + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ) + components.append( + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", + name="voxel_based_compare_map_filter", + remappings=[ + ("input", down_sample_topic), + ("map", "/map/pointcloud_map"), + ("output", LaunchConfiguration("output_topic")), + ], + parameters=[ + { + "distance_threshold": self.distance_threshold, + } + ], + extra_arguments=[ + {"use_intra_process_comms": False}, + ], + ) + ) + return components + + +def launch_setup(context, *args, **kwargs): + pipeline = PointcloudMapFilterPipeline(context) + components = [] + components.extend(pipeline.create_pipeline()) + individual_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=components, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + pointcloud_container_loader = LoadComposableNodes( + composable_node_descriptions=components, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + return [individual_container, pointcloud_container_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("input_topic", "") + add_launch_arg("output_topic", "") + add_launch_arg("use_multithread", "False") + add_launch_arg("use_intra_process", "True") + add_launch_arg("use_pointcloud_container", "False") + add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("tier4_perception_launch_param_path", "tier4_perception_launch parameter path") + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml new file mode 100644 index 0000000..21de187 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml new file mode 100644 index 0000000..fd9d1ba --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml new file mode 100644 index 0000000..3591b28 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py new file mode 100644 index 0000000..a748b1b --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -0,0 +1,536 @@ +# 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. +import os + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +class GroundSegmentationPipeline: + def __init__(self, context): + self.context = context + self.vehicle_info = self.get_vehicle_info() + ground_segmentation_param_path = os.path.join( + LaunchConfiguration("tier4_perception_launch_param_path").perform(context), + "obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml", + ) + with open(ground_segmentation_param_path, "r") as f: + self.ground_segmentation_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + self.single_frame_obstacle_seg_output = ( + "/perception/obstacle_segmentation/single_frame/pointcloud_raw" + ) + self.output_topic = "/perception/obstacle_segmentation/pointcloud" + self.use_single_frame_filter = self.ground_segmentation_param["use_single_frame_filter"] + self.use_time_series_filter = self.ground_segmentation_param["use_time_series_filter"] + + def get_vehicle_info(self): + path = LaunchConfiguration("vehicle_param_file").perform(self.context) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + p["vehicle_length"] = p["front_overhang"] + p["wheel_base"] + p["rear_overhang"] + p["vehicle_width"] = p["wheel_tread"] + p["left_overhang"] + p["right_overhang"] + p["min_longitudinal_offset"] = -p["rear_overhang"] + p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"] + p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"]) + p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"] + return p + + def get_vehicle_mirror_info(self): + path = LaunchConfiguration("vehicle_mirror_param_file").perform(self.context) + with open(path, "r") as f: + p = yaml.safe_load(f) + return p + + def create_additional_pipeline(self, lidar_name): + components = [] + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name=f"{lidar_name}_crop_box_filter", + remappings=[ + ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"), + ("output", f"{lidar_name}/range_cropped/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + }, + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="ground_segmentation", + plugin=self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["plugin"], + name=f"{lidar_name}_ground_filter", + remappings=[ + ("input", f"{lidar_name}/range_cropped/pointcloud"), + ("output", f"{lidar_name}/pointcloud"), + ], + parameters=[ + self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"] + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + def create_ransac_pipeline(self): + components = [] + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + namespace="plane_fitting", + remappings=[("output", "concatenated/pointcloud")], + parameters=[ + { + "input_topics": self.ground_segmentation_param["ransac_input_topics"], + "output_frame": LaunchConfiguration("base_frame"), + "timeout_sec": 1.0, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="short_height_obstacle_detection_area_filter", + namespace="plane_fitting", + remappings=[ + ("input", "concatenated/pointcloud"), + ("output", "detection_area/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + }, + self.ground_segmentation_param["short_height_obstacle_detection_area_filter"][ + "parameters" + ], + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::Lanelet2MapFilterComponent", + name="vector_map_filter", + namespace="plane_fitting", + remappings=[ + ("input/pointcloud", "detection_area/pointcloud"), + ("input/vector_map", "/map/vector_map"), + ("output", "vector_map_filtered/pointcloud"), + ], + parameters=[ + { + "voxel_size_x": 0.25, + "voxel_size_y": 0.25, + } + ], + # cannot use intra process because vector map filter uses transient local. + extra_arguments=[{"use_intra_process_comms": False}], + ) + ) + + components.append( + ComposableNode( + package="ground_segmentation", + plugin="ground_segmentation::RANSACGroundFilterComponent", + name="ransac_ground_filter", + namespace="plane_fitting", + remappings=[ + ("input", "vector_map_filtered/pointcloud"), + ("output", "pointcloud"), + ], + parameters=[self.ground_segmentation_param["ransac_ground_filter"]["parameters"]], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + def create_common_pipeline(self, input_topic, output_topic): + components = [] + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter", + remappings=[ + ("input", input_topic), + ("output", "range_cropped/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + }, + self.ground_segmentation_param["common_crop_box_filter"]["parameters"], + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="ground_segmentation", + plugin=self.ground_segmentation_param["common_ground_filter"]["plugin"], + name="common_ground_filter", + remappings=[ + ("input", "range_cropped/pointcloud"), + ("output", output_topic), + ], + parameters=[ + self.ground_segmentation_param["common_ground_filter"]["parameters"], + self.vehicle_info, + {"input_frame": "base_link"}, + {"output_frame": "base_link"}, + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + return components + + def create_single_frame_obstacle_segmentation_components(self, input_topic, output_topic): + + additional_lidars = self.ground_segmentation_param["additional_lidars"] + use_ransac = bool(self.ground_segmentation_param["ransac_input_topics"]) + use_additional = bool(additional_lidars) + relay_topic = "all_lidars/pointcloud" + common_pipeline_output = ( + "single_frame/pointcloud" if use_additional or use_ransac else output_topic + ) + + components = self.create_common_pipeline( + input_topic=input_topic, + output_topic=common_pipeline_output, + ) + + if use_additional: + for lidar_name in additional_lidars: + components.extend(self.create_additional_pipeline(lidar_name)) + components.append( + self.get_additional_lidars_concatenated_component( + input_topics=[common_pipeline_output] + + list(map(lambda x: f"{x}/pointcloud"), additional_lidars), + output_topic=relay_topic if use_ransac else output_topic, + ) + ) + + if use_ransac: + components.extend(self.create_ransac_pipeline()) + components.append( + self.get_single_frame_obstacle_segmentation_concatenated_component( + input_topics=[ + "plane_fitting/pointcloud", + relay_topic if use_additional else common_pipeline_output, + ], + output_topic=output_topic, + ) + ) + + return components + + @staticmethod + def create_time_series_outlier_filter_components(input_topic, output_topic): + components = [] + components.append( + ComposableNode( + package="occupancy_grid_map_outlier_filter", + plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", + name="occupancy_grid_map_outlier_filter", + remappings=[ + ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), + ("~/input/pointcloud", input_topic), + ("~/output/pointcloud", output_topic), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + @staticmethod + def create_single_frame_outlier_filter_components(input_topic, output_topic, context): + components = [] + components.append( + ComposableNode( + package="elevation_map_loader", + plugin="ElevationMapLoaderNode", + name="elevation_map_loader", + namespace="elevation_map", + remappings=[ + ("output/elevation_map", "map"), + ("input/pointcloud_map", "/map/pointcloud_map"), + ("input/vector_map", "/map/vector_map"), + ], + parameters=[ + { + "use_lane_filter": False, + "use_inpaint": True, + "inpaint_radius": 1.0, + "param_file_path": PathJoinSubstitution( + [ + LaunchConfiguration("tier4_perception_launch_param_path").perform( + context + ), + "obstacle_segmentation", + "ground_segmentation", + "elevation_map_parameters.yaml", + ] + ), + "elevation_map_directory": PathJoinSubstitution( + [FindPackageShare("elevation_map_loader"), "data", "elevation_maps"] + ), + "use_elevation_map_cloud_publisher": False, + } + ], + extra_arguments=[{"use_intra_process_comms": False}], + ) + ) + + components.append( + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::CompareElevationMapFilterComponent", + name="compare_elevation_map_filter", + namespace="elevation_map", + remappings=[ + ("input", input_topic), + ("output", "map_filtered/pointcloud"), + ("input/elevation_map", "map"), + ], + parameters=[ + { + "map_frame": "map", + "map_layer_name": "elevation", + "height_diff_thresh": 0.15, + "input_frame": "map", + "output_frame": "base_link", + } + ], + extra_arguments=[ + {"use_intra_process_comms": False} + ], # can't use this with transient_local + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + name="voxel_grid_filter", + namespace="elevation_map", + remappings=[ + ("input", "map_filtered/pointcloud"), + ("output", "voxel_grid_filtered/pointcloud"), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("base_frame"), + "voxel_size_x": 0.04, + "voxel_size_y": 0.04, + "voxel_size_z": 0.08, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", + name="voxel_grid_outlier_filter", + namespace="elevation_map", + remappings=[ + ("input", "voxel_grid_filtered/pointcloud"), + ("output", output_topic), + ], + parameters=[ + { + "voxel_size_x": 0.4, + "voxel_size_y": 0.4, + "voxel_size_z": 100.0, + "voxel_points_threshold": 5, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + return components + + @staticmethod + def get_additional_lidars_concatenated_component(input_topics, output_topic): + + return ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + remappings=[("output", output_topic)], + parameters=[ + { + "input_topics": input_topics, + "output_frame": LaunchConfiguration("base_frame"), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + @staticmethod + def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, output_topic): + return ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_no_ground_data", + remappings=[("output", output_topic)], + parameters=[ + { + "input_topics": input_topics, + "output_frame": LaunchConfiguration("base_frame"), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + +def launch_setup(context, *args, **kwargs): + pipeline = GroundSegmentationPipeline(context) + + components = [] + components.extend( + pipeline.create_single_frame_obstacle_segmentation_components( + input_topic=LaunchConfiguration("input/pointcloud"), + output_topic=pipeline.single_frame_obstacle_seg_output, + ) + ) + + relay_topic = "single_frame/filtered/pointcloud" + if pipeline.use_single_frame_filter: + components.extend( + pipeline.create_single_frame_outlier_filter_components( + input_topic=pipeline.single_frame_obstacle_seg_output, + output_topic=relay_topic + if pipeline.use_time_series_filter + else pipeline.output_topic, + context=context, + ) + ) + if pipeline.use_time_series_filter: + components.extend( + pipeline.create_time_series_outlier_filter_components( + input_topic=relay_topic + if pipeline.use_single_frame_filter + else pipeline.single_frame_obstacle_seg_output, + output_topic=pipeline.output_topic, + ) + ) + individual_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=components, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + pointcloud_container_loader = LoadComposableNodes( + composable_node_descriptions=components, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + return [individual_container, pointcloud_container_loader] + + +def generate_launch_description(): + + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("base_frame", "base_link") + add_launch_arg("vehicle_param_file") + add_launch_arg("use_multithread", "False") + add_launch_arg("use_intra_process", "True") + add_launch_arg("use_pointcloud_container", "False") + add_launch_arg("container_name", "perception_pipeline_container") + add_launch_arg("tier4_perception_launch_param_path", "tier4_perception_launch parameter path") + add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py new file mode 100644 index 0000000..d371fa4 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py @@ -0,0 +1,138 @@ +# Copyright 2021 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="pointcloud_to_laserscan", + plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode", + name="pointcloud_to_laserscan_node", + remappings=[ + ("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/output/laserscan", LaunchConfiguration("output/laserscan")), + ("~/output/pointcloud", LaunchConfiguration("output/pointcloud")), + ("~/output/ray", LaunchConfiguration("output/ray")), + ("~/output/stixel", LaunchConfiguration("output/stixel")), + ], + parameters=[ + { + "target_frame": "base_link", # Leave disabled to output scan in pointcloud frame + "transform_tolerance": 0.01, + "min_height": 0.0, + "max_height": 2.0, + "angle_min": -3.141592, # -M_PI + "angle_max": 3.141592, # M_PI + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 + "scan_time": 0.0, + "range_min": 1.0, + "range_max": 200.0, + "use_inf": True, + "inf_epsilon": 1.0, + # Concurrency level, affects number of pointclouds queued for processing + # and number of threads used + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + "concurrency_level": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", + name="occupancy_grid_map_node", + remappings=[ + ("~/input/laserscan", LaunchConfiguration("output/laserscan")), + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), + ], + parameters=[ + { + "map_resolution": 0.5, + "use_height_filter": True, + "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), + "input_obstacle_and_raw_pointcloud": LaunchConfiguration( + "input_obstacle_and_raw_pointcloud" + ), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ] + + occupancy_grid_map_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return LaunchDescription( + [ + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "false"), + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), + add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), + add_launch_arg("output", "occupancy_grid"), + add_launch_arg("output/laserscan", "virtual_scan/laserscan"), + add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"), + add_launch_arg("output/ray", "virtual_scan/ray"), + add_launch_arg("output/stixel", "virtual_scan/stixel"), + add_launch_arg("input_obstacle_pointcloud", "false"), + add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), + add_launch_arg("use_pointcloud_container", "False"), + add_launch_arg("container_name", "occupancy_grid_map_container"), + set_container_executable, + set_container_mt_executable, + occupancy_grid_map_container, + load_composable_nodes, + ] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py new file mode 100644 index 0000000..2b7f9f8 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py @@ -0,0 +1,93 @@ +# Copyright 2021 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + name="occupancy_grid_map_node", + remappings=[ + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), + ], + parameters=[ + { + "map_resolution": 0.5, + "use_height_filter": True, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ] + + occupancy_grid_map_container = ComposableNodeContainer( + condition=LaunchConfigurationEquals("container", ""), + name="occupancy_grid_map_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals("container", ""), + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container"), + ) + + return LaunchDescription( + [ + add_launch_arg("container", ""), + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "false"), + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), + add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), + add_launch_arg("output", "occupancy_grid"), + set_container_executable, + set_container_mt_executable, + occupancy_grid_map_container, + load_composable_nodes, + ] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/perception.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/perception.launch.xml new file mode 100644 index 0000000..c568ea6 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/perception.launch.xml @@ -0,0 +1,139 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml new file mode 100644 index 0000000..c429a0b --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py new file mode 100644 index 0000000..8993530 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -0,0 +1,201 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + ssd_fine_detector_share_dir = get_package_share_directory("traffic_light_ssd_fine_detector") + classifier_share_dir = get_package_share_directory("traffic_light_classifier") + add_launch_arg("enable_fine_detection", "True") + add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") + + # traffic_light_ssd_fine_detector + add_launch_arg( + "onnx_file", os.path.join(ssd_fine_detector_share_dir, "data", "mb2-ssd-lite-tlr.onnx") + ) + add_launch_arg( + "label_file", os.path.join(ssd_fine_detector_share_dir, "data", "voc_labels_tl.txt") + ) + add_launch_arg("fine_detector_precision", "FP32") + add_launch_arg("score_thresh", "0.7") + add_launch_arg("max_batch_size", "8") + add_launch_arg("approximate_sync", "False") + add_launch_arg("mean", "[0.5, 0.5, 0.5]") + add_launch_arg("std", "[0.5, 0.5, 0.5]") + + # traffic_light_classifier + add_launch_arg("classifier_type", "1") + add_launch_arg( + "model_file_path", + os.path.join(classifier_share_dir, "data", "traffic_light_classifier_mobilenetv2.onnx"), + ) + add_launch_arg("label_file_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt")) + add_launch_arg("precision", "fp16") + add_launch_arg("input_c", "3") + add_launch_arg("input_h", "224") + add_launch_arg("input_w", "224") + + add_launch_arg("use_intra_process", "False") + add_launch_arg("use_multithread", "False") + + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + container = ComposableNodeContainer( + name="traffic_light_node_container", + namespace="/perception/traffic_light_recognition", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + ComposableNode( + package="image_transport_decompressor", + plugin="image_preprocessor::ImageTransportDecompressor", + name="traffic_light_image_decompressor", + parameters=[{"encoding": "rgb8"}], + remappings=[ + ( + "~/input/compressed_image", + [LaunchConfiguration("input/image"), "/compressed"], + ), + ("~/output/raw_image", LaunchConfiguration("input/image")), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="traffic_light_classifier", + plugin="traffic_light::TrafficLightClassifierNodelet", + name="traffic_light_classifier", + parameters=[ + create_parameter_dict( + "approximate_sync", + "classifier_type", + "model_file_path", + "label_file_path", + "precision", + "input_c", + "input_h", + "input_w", + ) + ], + remappings=[ + ("~/input/image", LaunchConfiguration("input/image")), + ("~/input/rois", "rois"), + ("~/output/traffic_signals", "traffic_signals"), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="traffic_light_visualization", + plugin="traffic_light::TrafficLightRoiVisualizerNodelet", + name="traffic_light_roi_visualizer", + parameters=[create_parameter_dict("enable_fine_detection")], + remappings=[ + ("~/input/image", LaunchConfiguration("input/image")), + ("~/input/rois", "rois"), + ("~/input/rough/rois", "rough/rois"), + ("~/input/traffic_signals", "traffic_signals"), + ("~/output/image", "debug/rois"), + ("~/output/image/compressed", "debug/rois/compressed"), + ("~/output/image/compressedDepth", "debug/rois/compressedDepth"), + ("~/output/image/theora", "debug/rois/theora"), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ], + output="both", + ) + + ssd_fine_detector_param = create_parameter_dict( + "onnx_file", + "label_file", + "score_thresh", + "max_batch_size", + "approximate_sync", + "mean", + "std", + ) + ssd_fine_detector_param["mode"] = LaunchConfiguration("fine_detector_precision") + + loader = LoadComposableNodes( + composable_node_descriptions=[ + ComposableNode( + package="traffic_light_ssd_fine_detector", + plugin="traffic_light::TrafficLightSSDFineDetectorNodelet", + name="traffic_light_ssd_fine_detector", + parameters=[ssd_fine_detector_param], + remappings=[ + ("~/input/image", LaunchConfiguration("input/image")), + ("~/input/rois", "rough/rois"), + ("~/output/rois", "rois"), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("enable_fine_detection")), + ) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return LaunchDescription( + [ + *launch_arguments, + set_container_executable, + set_container_mt_executable, + container, + loader, + ] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/package.xml new file mode 100644 index 0000000..ae5c5df --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/package.xml @@ -0,0 +1,44 @@ + + + + tier4_perception_launch + 0.1.0 + The tier4_perception_launch package + Yukihiro Saito + Shunsuke Miura + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + compare_map_segmentation + detected_object_feature_remover + detected_object_validation + detection_by_tracker + euclidean_cluster + ground_segmentation + image_projection_based_fusion + image_transport_decompressor + lidar_apollo_instance_segmentation + map_based_prediction + multi_object_tracker + object_merger + object_range_splitter + occupancy_grid_map_outlier_filter + pointcloud_preprocessor + pointcloud_to_laserscan + shape_estimation + tensorrt_yolo + traffic_light_classifier + traffic_light_map_based_detector + traffic_light_ssd_fine_detector + traffic_light_visualization + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/perception_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/perception_launch.drawio.svg new file mode 100644 index 0000000..9daf008 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_perception_launch/perception_launch.drawio.svg @@ -0,0 +1,309 @@ + + + + + + + + + + + +
+
+
+ perception.launch.xml +
+
+
+ + package: tier4_perception_launch + +
+
+
+
+
+ + perception.launch.xml... + +
+
+ + + + + + + +
+
+
+ True +
+
+
+
+ + True + +
+
+ + + + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+
+ + launch name... + +
+
+ + + + +
+
+
+ ex: +
+
+
+
+ + ex: + +
+
+ + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+
+ + node name... + +
+
+ + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+
+ + other name... + +
+
+ + + + +
+
+
+ $(var use_empty_dynamic_object_publisher) +
+
+
+
+ + $(var use_empty_dynamic_object_publisher) + +
+
+ + + + +
+
+
+ detection.launch.xml +
+
+
+ + package: tier4_perception_launch + +
+
+
+
+
+ + detection.launch.xml... + +
+
+ + + + + +
+
+
+ False +
+
+
+
+ + False + +
+
+ + + + +
+
+
+ tracking.launch.xml +
+
+
+ + package: tier4_perception_launch + +
+
+
+
+
+ + tracking.launch.xml... + +
+
+ + + + +
+
+
+ prediction.launch.xml +
+
+
+ + package: tier4_perception_launch + +
+
+
+
+
+ + prediction.launch.xml... + +
+
+ + + + +
+
+
+ empty_objects_publisher +
+
+
+ + package: dummy_perception_publisher + +
+
+
+
+
+ + empty_objects_publisher... + +
+
+ + + + +
+
+
+ traffic_light_recognition.launch.xml +
+
+
+ + package: tier4_perception_launch + +
+
+
+
+
+ + traffic_light_recognition.launch.xml... + +
+
+ + +
+ + + + + Viewer does not support full SVG 1.1 + + + +
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/CMakeLists.txt new file mode 100644 index 0000000..306b557 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_planning_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/README.md new file mode 100644 index 0000000..2b57905 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/README.md @@ -0,0 +1,16 @@ +# tier4_planning_launch + +## Structure + +![tier4_planning_launch](./planning_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +```xml + + +``` diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/common.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/common.param.yaml new file mode 100644 index 0000000..a23570a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/common.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # constraints param for normal driving + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml new file mode 100644 index 0000000..329714e --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + resample: + ds_resample: 0.1 + num_resample: 1 + delta_yaw_threshold: 0.785 + + latacc: + enable_constant_velocity_while_turning: false + constant_velocity_dist_threshold: 2.0 + + forward: + max_acc: 1.0 + min_acc: -1.0 + max_jerk: 0.3 + min_jerk: -0.3 + kp: 0.3 + + backward: + start_jerk: -0.1 + min_jerk_mild_stop: -0.3 + min_jerk: -1.5 + min_acc_mild_stop: -1.0 + min_acc: -2.5 + span_jerk: -0.01 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml new file mode 100644 index 0000000..a6906b7 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + jerk_weight: 0.1 # weight for "smoothness" cost for jerk + over_v_weight: 10000.0 # weight for "over speed limit" cost + over_a_weight: 500.0 # weight for "over accel limit" cost + over_j_weight: 200.0 # weight for "over jerk limit" cost + jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml new file mode 100644 index 0000000..c993978 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 100.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 1000.0 # weight for "over accel limit" cost diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml new file mode 100644 index 0000000..ec38010 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 200.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 5000.0 # weight for "over accel limit" cost diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml new file mode 100644 index 0000000..91bc39b --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -0,0 +1,57 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] + stop_decel: 0.0 # deceleration at a stop point[m/ss] + + # external velocity limit parameter + margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] + + # curve parameters + max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] + min_curve_velocity: 0.5 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] + decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit + decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit + + # engage & replan parameters + replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] + engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) + engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # stop velocity + stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s] + stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. + + # path extraction parameters + extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m] + extract_behind_dist: 5.0 # backward trajectory distance used for planning [m] + delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian] + + # resampling parameters for optimization + max_trajectory_length: 200.0 # max trajectory length for resampling [m] + min_trajectory_length: 150.0 # min trajectory length for resampling [m] + resample_time: 5.0 # resample total time for dense sampling [s] + dense_resample_dt: 0.1 # resample time interval for dense sampling [s] + dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + sparse_resample_dt: 0.5 # resample time interval for sparse sampling [s] + sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m] + + # resampling parameters for post process + post_max_trajectory_length: 300.0 # max trajectory length for resampling [m] + post_min_trajectory_length: 30.0 # min trajectory length for resampling [m] + post_resample_time: 10.0 # resample total time for dense sampling [s] + post_dense_resample_dt: 0.1 # resample time interval for dense sampling [s] + post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] + post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] + + # steering angle rate limit parameters + max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] + resample_ds: 0.1 # distance between trajectory points [m] + curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] + curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] + + # system + over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml new file mode 100644 index 0000000..eb67097 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml new file mode 100644 index 0000000..d691228 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -0,0 +1,61 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + ros__parameters: + avoidance: + resample_interval_for_planning: 0.3 + resample_interval_for_output: 4.0 + detection_area_right_expand_dist: 0.0 + detection_area_left_expand_dist: 1.0 + + enable_avoidance_over_same_direction: true + enable_avoidance_over_opposite_direction: true + + threshold_distance_object_is_on_center: 1.0 # [m] + threshold_speed_object_is_stopped: 1.0 # [m/s] + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] + + prepare_time: 2.0 # [s] + min_prepare_distance: 1.0 # [m] + min_avoidance_distance: 10.0 # [m] + + min_nominal_avoidance_speed: 7.0 # [m/s] + min_sharp_avoidance_speed: 1.0 # [m/s] + + road_shoulder_safety_margin: 0.5 # [m] + + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + + nominal_lateral_jerk: 0.2 # [m/s3] + max_lateral_jerk: 1.0 # [m/s3] + + object_hold_max_count: 20 + + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] + + # for debug + publish_debug_marker: false + print_debug_info: false + + # not enabled yet + longitudinal_collision_margin_min_distance: 0.0 # [m] + longitudinal_collision_margin_time: 0.0 + + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: false + pedestrian: false + + # ---------- advanced parameters ---------- + avoidance_execution_lateral_threshold: 0.499 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml new file mode 100644 index 0000000..90a6432 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + backward_path_length: 5.0 + forward_path_length: 300.0 + backward_length_buffer_for_end_of_lane: 5.0 + backward_length_buffer_for_end_of_pull_over: 5.0 + backward_length_buffer_for_end_of_pull_out: 5.0 + minimum_lane_change_length: 12.0 + minimum_pull_over_length: 16.0 + drivable_area_resolution: 0.1 + drivable_lane_forward_length: 50.0 + drivable_lane_backward_length: 5.0 + drivable_lane_margin: 3.0 + drivable_area_margin: 6.0 + refine_goal_search_radius_range: 7.5 + intersection_search_distance: 30.0 + path_interval: 2.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml new file mode 100644 index 0000000..1a66bc0 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + lane_change: + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + lane_change_prepare_duration: 4.0 + lane_changing_duration: 8.0 + lane_change_finish_judge_buffer: 3.0 + minimum_lane_change_velocity: 5.6 + prediction_duration: 8.0 + prediction_time_resolution: 0.5 + static_obstacle_velocity_thresh: 1.5 + maximum_deceleration: 1.0 + enable_abort_lane_change: true + enable_collision_check_at_prepare_phase: true + use_predicted_path_outside_lanelet: true + use_all_predicted_path: true + enable_blocked_by_obstacle: false diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml new file mode 100644 index 0000000..f685f8a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + lane_following: + expand_drivable_area: false + right_bound_offset: 0.5 + left_bound_offset: 0.5 + lane_change_prepare_duration: 2.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml new file mode 100644 index 0000000..7a9a63f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -0,0 +1,30 @@ +/**: + ros__parameters: + pull_out: + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 + pull_out_finish_judge_buffer: 1.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + before_pull_out_straight_distance: 0.0 + minimum_shift_pull_out_distance: 20.0 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + # geometric pull out + enable_geometric_pull_out: true + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 15.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml new file mode 100644 index 0000000..44d859a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -0,0 +1,61 @@ +/**: + ros__parameters: + pull_over: + request_length: 200.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. + pull_over_velocity: 3.0 + pull_over_minimum_velocity: 1.38 + margin_from_boundary: 0.5 + decide_path_distance: 10.0 + maximum_deceleration: 1.0 + # goal research + enable_goal_research: true + search_priority: "efficient_path" # "efficient_path" or "close_goal" + forward_goal_search_length: 20.0 + backward_goal_search_length: 20.0 + goal_search_interval: 2.0 + longitudinal_margin: 3.0 + # occupancy grid map + use_occupancy_grid: true + use_occupancy_grid_for_longitudinal_margin: false + occupancy_grid_collision_check_margin: 0.0 + theta_size: 360 + obstacle_threshold: 60 + # object recognition + use_object_recognition: true + object_recognition_collision_check_margin: 1.0 + # shift path + enable_shift_parking: true + pull_over_sampling_num: 4 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + after_pull_over_straight_distance: 5.0 + before_pull_over_straight_distance: 5.0 + # parallel parking path + enable_arc_forward_parking: true + enable_arc_backward_parking: true + after_forward_parking_straight_distance: 2.0 + after_backward_parking_straight_distance: 2.0 + forward_parking_velocity: 1.38 + backward_parking_velocity: -1.38 + forward_parking_lane_departure_margin: 0.0 + backward_parking_lane_departure_margin: 0.0 + arc_path_interval: 1.0 + pull_over_max_steer_angle: 0.35 # 20deg + # hazard on when parked + hazard_on_threshold_distance: 1.0 + hazard_on_threshold_velocity: 0.5 + # check safety with dynamic objects. Not used now. + pull_over_duration: 2.0 + pull_over_prepare_duration: 4.0 + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + enable_collision_check_at_prepare_phase: false + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + # debug + print_debug_info: false diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml new file mode 100644 index 0000000..7904404 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + side_shift: + min_distance_to_start_shifting: 5.0 + time_to_start_shifting: 1.0 + shifting_lateral_jerk: 0.2 + min_shifting_distance: 5.0 + min_shifting_speed: 5.56 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml new file mode 100644 index 0000000..1e0777e --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + launch_stop_line: true + launch_crosswalk: true + launch_traffic_light: true + launch_intersection: true + launch_blind_spot: true + launch_detection_area: true + launch_virtual_traffic_light: true + launch_occlusion_spot: true + launch_no_stopping_area: true + launch_run_out: false + forward_path_length: 1000.0 + backward_path_length: 5.0 + max_accel: -2.8 + max_jerk: -5.0 + system_delay: 0.5 + delay_response_time: 0.5 + is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml new file mode 100644 index 0000000..31f75d7 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + blind_spot: + use_pass_judge_line: true + stop_line_margin: 1.0 # [m] + backward_length: 15.0 # [m] + ignore_width_from_center_line: 0.7 # [m] + max_future_movement_time: 10.0 # [second] diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml new file mode 100644 index 0000000..2cc5260 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -0,0 +1,43 @@ +/**: + ros__parameters: + crosswalk: + show_processing_time: false # [-] whether to show processing time + + # param for stop position + stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists + stop_line_margin: 10.0 # [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_margin meters before the object) + stop_margin: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk + + # param for ego velocity + min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) + max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake + max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake + no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) + + # param for stuck vehicle + stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck + max_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked + stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk + + # param for pass judge logic + ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) + min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) + max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + + # param for input data + tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + + # param for target area & object + crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + target_object: + unknown: true # [-] whether to look and stop by UNKNOWN objects + bicycle: true # [-] whether to look and stop by BICYCLE objects + motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) + pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects + + walkway: + stop_duration_sec: 1.0 # [s] stop time at stop position + stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml new file mode 100644 index 0000000..9174045 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + detection_area: + stop_margin: 0.0 + use_dead_line: false + dead_line_margin: 5.0 + use_pass_judge_line: false + state_clear_time: 2.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml new file mode 100644 index 0000000..79eb257 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + intersection: + state_transit_margin_time: 0.5 + stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr + keep_detection_vel_thr: 0.833 # == 3.0km/h + stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. + stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h + intersection_velocity: 2.778 # 2.778m/s = 10.0km/h + intersection_max_accel: 0.5 # m/ss + detection_area_margin: 0.5 # [m] + detection_area_right_margin: 0.5 # [m] + detection_area_left_margin: 0.5 # [m] + detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] + min_predicted_path_confidence: 0.05 + collision_start_margin_time: 5.0 # [s] + collision_end_margin_time: 2.0 # [s] + use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck + assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning + enable_front_car_decel_prediction: false # By default this feature is disabled + + merge_from_private_road: + stop_duration_sec: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml new file mode 100644 index 0000000..32cd05a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + no_stopping_area: + state_clear_time: 2.0 # [s] time to clear stop state + stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h + stop_margin: 0.0 # [m] margin to stop line at no stopping area + dead_line_margin: 1.0 # [m] dead line offset go at no stopping area + stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area + detection_area_length: 200.0 # [m] used to create detection area polygon + stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml new file mode 100644 index 0000000..957f798 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml @@ -0,0 +1,34 @@ +/**: + ros__parameters: + occlusion_spot: + detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" + pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity" + use_object_info: true # [-] whether to reflect object info to occupancy grid map or not + use_moving_object_ray_cast: true # [-] whether to reflect moving object ray_cast to occupancy grid map or not + use_partition_lanelet: true # [-] whether to use partition lanelet map data + pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity + pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m) + debug: # !Note: default should be false for performance + is_show_occlusion: false # [-] whether to show occlusion point markers. + is_show_cv_window: false # [-] whether to show open_cv debug window + is_show_processing_time: false # [-] whether to show processing time + threshold: + detection_area_length: 100.0 # [m] the length of path to consider perception range + stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop + lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision + motion: + safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety + max_slow_down_jerk: -0.5 # [m/s^3] minimum jerk deceleration for safe brake. + max_slow_down_accel: -1.8 # [m/s^2] minimum accel deceleration for safe brake. + non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning. + non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning. + min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed + safe_margin: 2.0 # [m] margin for detection failure(0.5m) + pedestrian radius(0.5m) + safe margin(1.0m) + detection_area: + min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. + slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. + min_longitudinal_offset: 1.0 # [m] detection area safety buffer from front bumper. + max_lateral_distance: 6.0 # [m] buffer around the ego path used to build the detection area. + grid: + free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid + occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml new file mode 100644 index 0000000..ccf90a0 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -0,0 +1,45 @@ +/**: + ros__parameters: + run_out: + detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points + use_partition_lanelet: true # [-] whether to use partition lanelet map data + specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin + passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin + deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles + detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles + detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time + min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision + + detection_area: + margin_behind: 0.5 # [m] ahead margin for detection area length + margin_ahead: 1.0 # [m] behind margin for detection area length + + # parameter to create abstracted dynamic obstacles + dynamic_obstacle: + min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles + max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles + diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points + height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points + max_prediction_time: 10.0 # [sec] create predicted path until this time + time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path + points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method + + # approach if ego has stopped in the front of the obstacle for a certain amount of time + approaching: + enable: false + margin: 0.0 # [m] distance on how close ego approaches the obstacle + limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + + # parameters for the change of state. used only when approaching is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value + keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition + + # parameter to avoid sudden stopping + slow_down_limit: + enable: true + max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. + max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml new file mode 100644 index 0000000..a2b5ac5 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + stop_line: + stop_margin: 0.0 + stop_check_dist: 2.0 + stop_duration_sec: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml new file mode 100644 index 0000000..f5db276 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + traffic_light: + stop_margin: 0.0 + tl_state_timeout: 1.0 + external_tl_state_timeout: 1.0 + yellow_lamp_period: 2.75 + enable_pass_judge: true diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml new file mode 100644 index 0000000..267dde5 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + virtual_traffic_light: + max_delay_sec: 3.0 + near_line_distance: 1.0 + dead_line_margin: 1.0 + max_yaw_deviation_deg: 90.0 + check_timeout_after_stop_line: true diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml new file mode 100644 index 0000000..0bf4238 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + backward_path_length: 5.0 + forward_path_length: 300.0 + max_accel: -5.0 + lane_change_prepare_duration: 4.0 + lane_changing_duration: 8.0 + backward_length_buffer_for_end_of_lane: 5.0 + lane_change_finish_judge_buffer: 3.0 + minimum_lane_change_length: 12.0 + minimum_lane_change_velocity: 5.6 + prediction_duration: 8.0 + prediction_time_resolution: 0.5 + drivable_area_resolution: 0.1 + drivable_area_width: 100.0 + drivable_area_height: 50.0 + static_obstacle_velocity_thresh: 1.5 + maximum_deceleration: 1.0 + refine_goal_search_radius_range: 7.5 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml new file mode 100644 index 0000000..8ba0a40 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml @@ -0,0 +1,29 @@ +/**: + ros__parameters: + module_list: + - "blind_spot" + - "crosswalk" + - "detection_area" + - "intersection" + - "no_stopping_area" + - "traffic_light" + - "lane_change_left" + - "lane_change_right" + - "avoidance_left" + - "avoidance_right" + - "pull_over" + - "pull_out" + + default_enable_list: + - "blind_spot" + - "crosswalk" + - "detection_area" + - "intersection" + - "no_stopping_area" + - "traffic_light" + - "lane_change_left" + - "lane_change_right" + - "avoidance_left" + - "avoidance_right" + - "pull_over" + - "pull_out" diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml new file mode 100644 index 0000000..50f5caa --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -0,0 +1,156 @@ +/**: + ros__parameters: + option: + # publish + is_publishing_debug_visualization_marker: true + is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid + is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid + is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid + + is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area + + # show + is_showing_debug_info: false + is_showing_calculation_time: false + + # other + enable_avoidance: false # enable avoidance function + enable_pre_smoothing: true # enable EB + skip_optimization: false # skip MPT and EB + reset_prev_optimization: false + + common: + # sampling + num_sampling_points: 100 # number of optimizing points + + # trajectory total/fixing length + trajectory_length: 300.0 # total trajectory length[m] + + forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] + forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] + + backward_fixing_distance: 5.0 # backward fixing length from base_link [m] + delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] + + delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] + delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point + delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + + num_fix_points_for_extending: 50 # number of fixing points when extending + max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] + + object: # avoiding object + max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] + max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] + + avoiding_object_type: + unknown: true + car: true + truck: true + bus: true + bicycle: true + motorbike: true + pedestrian: true + animal: true + + # mpt param + mpt: + option: + steer_limit_constraint: true + fix_points_around_ego: true + plan_from_ego: true + max_plan_from_ego_length: 10.0 + visualize_sampling_num: 1 + enable_manual_warm_start: true + enable_warm_start: true # false + is_fixed_point_single: false + + common: + num_curvature_sampling_points: 5 # number of sampling points when calculating curvature + delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] + + # kinematics: + # If this parameter is commented out, the parameter is set as below by default. + # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8` + # The 0.8 scale is adopted as it performed the best. + # optimization_center_offset: 2.3 # optimization center offset from base link + + # replanning & trimming trajectory param outside algorithm + replan: + max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] + max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] + max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] + + # advanced parameters to improve performance as much as possible + advanced: + eb: + common: + num_joint_buffer_points: 3 # number of joint buffer points + num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx + delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. + num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points + + clearance: + clearance_for_straight_line: 0.05 # minimum optimizing range around straight points + clearance_for_joint: 0.1 # minimum optimizing range around joint points + clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing + + qp: + max_iteration: 10000 # max iteration when solving QP + eps_abs: 1.0e-8 # eps abs when solving OSQP + eps_rel: 1.0e-10 # eps rel when solving OSQP + + mpt: + bounds_search_widths: [0.45, 0.15, 0.05, 0.01] + + clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory + hard_clearance_from_road: 0.0 # clearance from road boundary[m] + soft_clearance_from_road: 0.1 # clearance from road boundary[m] + soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] + clearance_from_object: 1.0 # clearance from object[m] + extra_desired_clearance_from_road: 0.0 # extra desired clearance from road + + weight: + soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point + soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point + + lat_error_weight: 100.0 # weight for lateral error + yaw_error_weight: 0.0 # weight for yaw error + yaw_error_rate_weight: 0.0 # weight for yaw error rate + steer_input_weight: 10.0 # weight for steering input + steer_rate_weight: 10.0 # weight for steering rate + + obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error + obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error + obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error + near_objects_length: 30.0 # weight for yaw error + + terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point + terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point + terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point + terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point + + # check if planned trajectory is outside drivable area + collision_free_constraints: + option: + l_inf_norm: true + soft_constraint: true + hard_constraint: false + # two_step_soft_constraint: false + + vehicle_circles: + method: "rear_drive" + + uniform_circle: + num: 3 + radius_ratio: 0.8 + + rear_drive: + num_for_calculation: 3 + front_radius_ratio: 1.0 + rear_radius_ratio: 1.0 + + bicycle_model: + num_for_calculation: 3 + front_radius_ratio: 1.0 + rear_radius_ratio: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml new file mode 100644 index 0000000..520637a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -0,0 +1,115 @@ +/**: + ros__parameters: + common: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + + is_showing_debug_info: true + + # longitudinal info + idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] + min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] + min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] + safe_distance_margin : 6.0 # This is also used as a stop margin [m] + terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index + min_behavior_stop_margin: 3.0 # [m] + obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + + cruise_obstacle_type: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: false + pedestrian: false + + stop_obstacle_type: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + obstacle_filtering: + rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m] + detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking + + # if crossing vehicle is decided as target obstacles or not + crossing_obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + + outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m] + outside_obstacle_min_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + + prediction_resampling_time_interval: 0.1 + prediction_resampling_time_horizon: 10.0 + + goal_extension_length: 5.0 # extension length for collision check around the goal + goal_extension_interval: 1.0 # extension interval for collision check around the goal + + stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle + + ignored_outside_obstacle_type: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: true + pedestrian: true + + pid_based_planner: + # use_predicted_obstacle_pose: false + + # PID gains to keep safe distance with the front vehicle + kp: 2.5 + ki: 0.0 + kd: 2.3 + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + + lpf_cruise_gain: 0.2 + + optimization_based_planner: + dense_resampling_time_interval: 0.2 + sparse_resampling_time_interval: 2.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + velocity_margin: 0.2 #[m/s] + + # Parameters for safe distance + t_dangerous: 0.5 + + # For initial Motion + replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] + engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) + engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # Weights for optimization + max_s_weight: 100.0 + max_v_weight: 1.0 + over_s_safety_weight: 1000000.0 + over_s_ideal_weight: 50.0 + over_v_weight: 500000.0 + over_a_weight: 5000.0 + over_j_weight: 10000.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml new file mode 100644 index 0000000..dd76f2c --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + adaptive_cruise_control: + # Adaptive Cruise Control (ACC) config + use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not + use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not + consider_obj_velocity: true # consider forward vehicle velocity to ACC or not + + # general parameter for ACC + obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] + obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] + emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] + min_dist_stop: 4.0 # minimum distance of emergency stop [m] + max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] + min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control + standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] + min_dist_standard: 4.0 # minimum distance in active cruise control [m] + obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] + margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] + + # pid parameter for ACC + p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] + p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] + d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] + d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] + + # parameter for object velocity estimation + object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] + object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] + valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] + valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] + valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] + valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] + thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] + lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity + use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) + rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml new file mode 100644 index 0000000..91433f7 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml @@ -0,0 +1,51 @@ +/**: + ros__parameters: + hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + lowpass_gain: 0.9 # gain parameter for low pass filter [-] + max_velocity: 20.0 # max velocity [m/s] + enable_slow_down: False # whether to use slow down planner [-] + enable_z_axis_obstacle_filtering: True + z_axis_filtering_buffer: 0.0 + + stop_planner: + # params for stop position + stop_position: + max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_longitudinal_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] + hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] + + # params for detection area + detection_area: + lateral_margin: 0.0 # margin of vehicle footprint [m] + step_length: 1.0 # step length for pointcloud search range [m] + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] + + + slow_down_planner: + # params for slow down section + slow_down_section: + longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] + longitudinal_backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m] + longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + + # params for detection area + detection_area: + lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + + # params for velocity + target_velocity: + max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s] + min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s] + slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s] + + # params for deceleration constraints (use this param if consider_constraints is True) + constraints: + jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss] + jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] + jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] + + # others + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] + velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] + acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml new file mode 100644 index 0000000..6aa4e71 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + + # obstacle check + use_pointcloud: true # use pointcloud as obstacle check + use_dynamic_object: true # use dynamic object as obstacle check + surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] + state_clear_time: 2.0 + + # ego stop state + stop_state_ego_speed: 0.1 #[m/s] + + # debug + publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml new file mode 100644 index 0000000..d6152c4 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml @@ -0,0 +1,39 @@ +/**: + ros__parameters: + # -- Node Configurations -- + planning_algorithm: "astar" + waypoints_velocity: 5.0 + update_rate: 10.0 + th_arrived_distance_m: 1.0 + th_stopped_time_sec: 1.0 + th_stopped_velocity_mps: 0.01 + th_course_out_distance_m: 1.0 + vehicle_shape_margin_m: 1.0 + replan_when_obstacle_found: true + replan_when_course_out: true + + # -- Configurations common to the all planners -- + # base configs + time_limit: 30000.0 + # robot configs # TODO replace by vehicle_info + robot_length: 4.5 + robot_width: 1.75 + robot_base2back: 1.0 + minimum_turning_radius: 9.0 + maximum_turning_radius: 9.0 + turning_radius_size: 1 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 2.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 100 + + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: true + distance_heuristic_weight: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml new file mode 100644 index 0000000..c1ace97 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/planning.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/planning.launch.xml new file mode 100644 index 0000000..e179d41 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/planning.launch.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml new file mode 100644 index 0000000..31f5820 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml new file mode 100644 index 0000000..7cd07bd --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py new file mode 100644 index 0000000..0dcfaf8 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -0,0 +1,509 @@ +# Copyright 2021 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. + +import os + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + + # vehicle information parameter + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # nearest search parameter + nearest_search_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "nearest_search.param.yaml", + ) + with open(nearest_search_param_path, "r") as f: + nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # behavior path planner + side_shift_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "side_shift", + "side_shift.param.yaml", + ) + with open(side_shift_param_path, "r") as f: + side_shift_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + avoidance_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "avoidance", + "avoidance.param.yaml", + ) + with open(avoidance_param_path, "r") as f: + avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + lane_change_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "lane_change", + "lane_change.param.yaml", + ) + with open(lane_change_param_path, "r") as f: + lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + lane_following_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "lane_following", + "lane_following.param.yaml", + ) + with open(lane_following_param_path, "r") as f: + lane_following_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + pull_over_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "pull_over", + "pull_over.param.yaml", + ) + with open(pull_over_param_path, "r") as f: + pull_over_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + pull_out_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "pull_out", + "pull_out.param.yaml", + ) + with open(pull_out_param_path, "r") as f: + pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + behavior_path_planner_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "behavior_path_planner.param.yaml", + ) + with open(behavior_path_planner_param_path, "r") as f: + behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + behavior_path_planner_component = ComposableNode( + package="behavior_path_planner", + plugin="behavior_path_planner::BehaviorPathPlannerNode", + name="behavior_path_planner", + namespace="", + remappings=[ + ("~/input/route", LaunchConfiguration("input_route_topic_name")), + ("~/input/vector_map", LaunchConfiguration("map_topic_name")), + ("~/input/perception", "/perception/object_recognition/objects"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/accel", "/localization/acceleration"), + ("~/input/scenario", "/planning/scenario_planning/scenario"), + ("~/output/path", "path_with_lane_id"), + ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"), + ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"), + ], + parameters=[ + nearest_search_param, + side_shift_param, + avoidance_param, + lane_change_param, + lane_following_param, + pull_over_param, + pull_out_param, + behavior_path_planner_param, + vehicle_info_param, + { + "bt_tree_config_path": [ + FindPackageShare("behavior_path_planner"), + "/config/behavior_path_planner_tree.xml", + ], + "planning_hz": 10.0, + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # smoother param + common_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "common.param.yaml", + ) + with open(common_param_path, "r") as f: + common_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + motion_velocity_smoother_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "motion_velocity_smoother", + "motion_velocity_smoother.param.yaml", + ) + with open(motion_velocity_smoother_param_path, "r") as f: + motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + smoother_type_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "motion_velocity_smoother", + "Analytical.param.yaml", + ) + with open(smoother_type_param_path, "r") as f: + smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # behavior velocity planner + blind_spot_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "blind_spot.param.yaml", + ) + with open(blind_spot_param_path, "r") as f: + blind_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + crosswalk_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "crosswalk.param.yaml", + ) + with open(crosswalk_param_path, "r") as f: + crosswalk_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + detection_area_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "detection_area.param.yaml", + ) + with open(detection_area_param_path, "r") as f: + detection_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + intersection_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "intersection.param.yaml", + ) + with open(intersection_param_path, "r") as f: + intersection_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + stop_line_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "stop_line.param.yaml", + ) + with open(stop_line_param_path, "r") as f: + stop_line_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + traffic_light_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "traffic_light.param.yaml", + ) + with open(traffic_light_param_path, "r") as f: + traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + virtual_traffic_light_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "virtual_traffic_light.param.yaml", + ) + with open(virtual_traffic_light_param_path, "r") as f: + virtual_traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + occlusion_spot_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "occlusion_spot.param.yaml", + ) + with open(occlusion_spot_param_path, "r") as f: + occlusion_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + no_stopping_area_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "no_stopping_area.param.yaml", + ) + with open(no_stopping_area_param_path, "r") as f: + no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + run_out_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "run_out.param.yaml", + ) + with open(run_out_param_path, "r") as f: + run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + behavior_velocity_planner_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "behavior_velocity_planner.param.yaml", + ) + with open(behavior_velocity_planner_param_path, "r") as f: + behavior_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + behavior_velocity_planner_component = ComposableNode( + package="behavior_velocity_planner", + plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode", + name="behavior_velocity_planner", + namespace="", + remappings=[ + ("~/input/path_with_lane_id", "path_with_lane_id"), + ("~/input/vector_map", "/map/vector_map"), + ("~/input/vehicle_odometry", "/localization/kinematic_state"), + ("~/input/accel", "/localization/acceleration"), + ("~/input/dynamic_objects", "/perception/object_recognition/objects"), + ( + "~/input/no_ground_pointcloud", + "/perception/obstacle_segmentation/pointcloud", + ), + ( + "~/input/compare_map_filtered_pointcloud", + "compare_map_filtered/pointcloud", + ), + ( + "~/input/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", + ), + ( + "~/input/external_traffic_signals", + "/external/traffic_light_recognition/traffic_signals", + ), + ( + "~/input/external_velocity_limit_mps", + "/planning/scenario_planning/max_velocity_default", + ), + ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"), + ( + "~/input/occupancy_grid", + "/perception/occupancy_grid_map/map", + ), + ("~/output/path", "path"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ( + "~/output/infrastructure_commands", + "/planning/scenario_planning/status/infrastructure_commands", + ), + ("~/output/traffic_signal", "debug/traffic_signal"), + ], + parameters=[ + nearest_search_param, + behavior_velocity_planner_param, + blind_spot_param, + crosswalk_param, + detection_area_param, + intersection_param, + stop_line_param, + traffic_light_param, + virtual_traffic_light_param, + occlusion_spot_param, + no_stopping_area_param, + vehicle_info_param, + run_out_param, + common_param, + motion_velocity_smoother_param, + smoother_type_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="behavior_planning_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + behavior_path_planner_component, + behavior_velocity_planner_component, + ], + output="screen", + ) + + # This condition is true if run_out module is enabled and its detection method is Points + launch_run_out_with_points_method = PythonExpression( + [ + LaunchConfiguration( + "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"] + ), + " and ", + "'", + run_out_param["run_out"]["detection_method"], + "' == 'Points'", + ] + ) + + # load compare map for run_out module + load_compare_map = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + FindPackageShare("tier4_planning_launch"), + "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py", + ], + ), + launch_arguments={ + "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"), + "container_name": LaunchConfiguration("container_name"), + "use_multithread": "true", + }.items(), + # launch compare map only when run_out module is enabled and detection method is Points + condition=IfCondition(launch_run_out_with_points_method), + ) + + load_vector_map_inside_area_filter = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + FindPackageShare("tier4_planning_launch"), + "/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py", + ] + ), + launch_arguments={ + "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"), + "container_name": LaunchConfiguration("container_name"), + "use_multithread": "true", + "polygon_type": "no_obstacle_segmentation_area_for_run_out", + }.items(), + # launch vector map filter only when run_out module is enabled and detection method is Points + condition=IfCondition(launch_run_out_with_points_method), + ) + + group = GroupAction( + [ + container, + load_compare_map, + load_vector_map_inside_area_filter, + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + add_launch_arg( + "input_route_topic_name", "/planning/mission_planning/route", "input topic of route" + ) + add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map") + + add_launch_arg("tier4_planning_launch_param_path", None, "tier4_planning_launch parameter path") + + # component + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + + # for points filter of run out module + add_launch_arg("use_pointcloud_container", "true") + add_launch_arg("container_name", "pointcloud_container") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml new file mode 100644 index 0000000..c4c7b3b --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py new file mode 100644 index 0000000..60b31f4 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py @@ -0,0 +1,89 @@ +# Copyright 2022 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent", + name="voxel_distance_based_compare_map_filter_node", + remappings=[ + ("input", "vector_map_inside_area_filtered/pointcloud"), + ("map", "/map/pointcloud_map"), + ("output", "compare_map_filtered/pointcloud"), + ], + parameters=[ + { + "distance_threshold": 0.7, + } + ], + extra_arguments=[ + {"use_intra_process_comms": False} # this node has QoS of transient local + ], + ), + ] + + compare_map_container = ComposableNodeContainer( + name="compare_map_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return LaunchDescription( + [ + add_launch_arg("use_multithread", "true"), + add_launch_arg("use_pointcloud_container", "true"), + add_launch_arg("container_name", "compare_map_container"), + set_container_executable, + set_container_mt_executable, + compare_map_container, + load_composable_nodes, + ] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py new file mode 100644 index 0000000..3921dcf --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py @@ -0,0 +1,88 @@ +# Copyright 2022 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent", + name="vector_map_inside_area_filter_node", + remappings=[ + ("input", "/perception/obstacle_segmentation/pointcloud"), + ("input/vector_map", "/map/vector_map"), + ("output", "vector_map_inside_area_filtered/pointcloud"), + ], + parameters=[ + { + "polygon_type": LaunchConfiguration("polygon_type"), + } + ], + # this node has QoS of transient local + extra_arguments=[{"use_intra_process_comms": False}], + ), + ] + + vector_map_area_filter_container = ComposableNodeContainer( + name="vector_map_area_filter_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return LaunchDescription( + [ + add_launch_arg("use_multithread", "true"), + add_launch_arg("use_pointcloud_container", "true"), + add_launch_arg("container_name", "vector_map_area_filter_container"), + set_container_executable, + set_container_mt_executable, + vector_map_area_filter_container, + load_composable_nodes, + ] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py new file mode 100644 index 0000000..44ff59d --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -0,0 +1,330 @@ +# Copyright 2021 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. + +import os + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + # vehicle information param path + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # planning common param path + common_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "common.param.yaml", + ) + with open(common_param_path, "r") as f: + common_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # nearest search parameter + nearest_search_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "nearest_search.param.yaml", + ) + with open(nearest_search_param_path, "r") as f: + nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # obstacle avoidance planner + obstacle_avoidance_planner_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_avoidance_planner", + "obstacle_avoidance_planner.param.yaml", + ) + with open(obstacle_avoidance_planner_param_path, "r") as f: + obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_avoidance_planner_component = ComposableNode( + package="obstacle_avoidance_planner", + plugin="ObstacleAvoidancePlanner", + name="obstacle_avoidance_planner", + namespace="", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/path", LaunchConfiguration("input_path_topic")), + ("~/output/path", "obstacle_avoidance_planner/trajectory"), + ], + parameters=[ + nearest_search_param, + obstacle_avoidance_planner_param, + vehicle_info_param, + {"is_showing_debug_info": False}, + {"is_stopping_if_outside_drivable_area": True}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # surround obstacle checker + surround_obstacle_checker_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "motion_planning", + "surround_obstacle_checker", + "surround_obstacle_checker.param.yaml", + ) + with open(surround_obstacle_checker_param_path, "r") as f: + surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + surround_obstacle_checker_component = ComposableNode( + package="surround_obstacle_checker", + plugin="surround_obstacle_checker::SurroundObstacleCheckerNode", + name="surround_obstacle_checker", + namespace="", + remappings=[ + ("~/output/no_start_reason", "/planning/scenario_planning/status/no_start_reason"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), + ( + "~/output/velocity_limit_clear_command", + "/planning/scenario_planning/clear_velocity_limit", + ), + ( + "~/input/pointcloud", + "/perception/obstacle_segmentation/pointcloud", + ), + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/odometry", "/localization/kinematic_state"), + ], + parameters=[ + surround_obstacle_checker_param, + vehicle_info_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # obstacle stop planner + obstacle_stop_planner_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_stop_planner", + "obstacle_stop_planner.param.yaml", + ) + obstacle_stop_planner_acc_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_stop_planner", + "adaptive_cruise_control.param.yaml", + ) + with open(obstacle_stop_planner_param_path, "r") as f: + obstacle_stop_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(obstacle_stop_planner_acc_param_path, "r") as f: + obstacle_stop_planner_acc_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_stop_planner_component = ComposableNode( + package="obstacle_stop_planner_custom", + plugin="motion_planning::ObstacleStopPlannerNode", + name="obstacle_stop_planner", + namespace="", + remappings=[ + ("~/output/stop_reason", "/planning/scenario_planning/status/stop_reason"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), + ( + "~/output/velocity_limit_clear_command", + "/planning/scenario_planning/clear_velocity_limit", + ), + ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), + ("~/input/acceleration", "/localization/acceleration"), + ( + "~/input/pointcloud", + "/perception/obstacle_segmentation/pointcloud", + ), + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ], + parameters=[ + nearest_search_param, + common_param, + obstacle_stop_planner_param, + obstacle_stop_planner_acc_param, + vehicle_info_param, + {"enable_slow_down": False}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # obstacle cruise planner + obstacle_cruise_planner_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_cruise_planner", + "obstacle_cruise_planner.param.yaml", + ) + with open(obstacle_cruise_planner_param_path, "r") as f: + obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_cruise_planner_component = ComposableNode( + package="obstacle_cruise_planner", + plugin="motion_planning::ObstacleCruisePlannerNode", + name="obstacle_cruise_planner", + namespace="", + remappings=[ + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/acceleration", "/localization/acceleration"), + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), + ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"), + ("~/output/clear_velocity_limit", "/planning/scenario_planning/clear_velocity_limit"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ], + parameters=[ + nearest_search_param, + common_param, + obstacle_cruise_planner_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + obstacle_cruise_planner_relay_component = ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="obstacle_cruise_planner_relay", + namespace="", + parameters=[ + {"input_topic": "obstacle_avoidance_planner/trajectory"}, + {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"}, + {"type": "autoware_auto_planning_msgs/msg/Trajectory"}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="motion_planning_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + obstacle_avoidance_planner_component, + ], + ) + + obstacle_stop_planner_loader = LoadComposableNodes( + composable_node_descriptions=[obstacle_stop_planner_component], + target_container=container, + condition=LaunchConfigurationEquals("cruise_planner", "obstacle_stop_planner"), + ) + + obstacle_cruise_planner_loader = LoadComposableNodes( + composable_node_descriptions=[obstacle_cruise_planner_component], + target_container=container, + condition=LaunchConfigurationEquals("cruise_planner", "obstacle_cruise_planner"), + ) + + obstacle_cruise_planner_relay_loader = LoadComposableNodes( + composable_node_descriptions=[obstacle_cruise_planner_relay_component], + target_container=container, + condition=LaunchConfigurationEquals("cruise_planner", "none"), + ) + + surround_obstacle_checker_loader = LoadComposableNodes( + composable_node_descriptions=[surround_obstacle_checker_component], + target_container=container, + condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), + ) + + group = GroupAction( + [ + container, + obstacle_stop_planner_loader, + obstacle_cruise_planner_loader, + obstacle_cruise_planner_relay_loader, + surround_obstacle_checker_loader, + ] + ) + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + # vehicle information parameter file + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + # obstacle_avoidance_planner + add_launch_arg( + "input_path_topic", + "/planning/scenario_planning/lane_driving/behavior_planning/path", + "input path topic of obstacle_avoidance_planner", + ) + + # surround obstacle checker + add_launch_arg("use_surround_obstacle_check", "true", "launch surround_obstacle_checker or not") + add_launch_arg( + "cruise_planner", "obstacle_stop_planner", "cruise planner type" + ) # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none" + + add_launch_arg("tier4_planning_launch_param_path", None, "tier4_planning_launch parameter path") + + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml new file mode 100644 index 0000000..1e77de2 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/parking.launch.py new file mode 100644 index 0000000..81758ee --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -0,0 +1,169 @@ +# Copyright 2021 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. + +import os + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + freespace_planner_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "parking", + "freespace_planner", + "freespace_planner.param.yaml", + ) + with open(freespace_planner_param_path, "r") as f: + freespace_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + container = ComposableNodeContainer( + name="parking_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + ComposableNode( + package="costmap_generator", + plugin="CostmapGenerator", + name="costmap_generator", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ( + "~/input/points_no_ground", + "/perception/obstacle_segmentation/pointcloud", + ), + ("~/input/vector_map", "/map/vector_map"), + ("~/input/scenario", "/planning/scenario_planning/scenario"), + ("~/output/grid_map", "costmap_generator/grid_map"), + ("~/output/occupancy_grid", "costmap_generator/occupancy_grid"), + ], + parameters=[ + { + "costmap_frame": "map", + "vehicle_frame": "base_link", + "map_frame": "map", + "update_rate": 10.0, + "use_wayarea": True, + "use_parkinglot": True, + "use_objects": True, + "use_points": True, + "grid_min_value": 0.0, + "grid_max_value": 1.0, + "grid_resolution": 0.2, + "grid_length_x": 70.0, + "grid_length_y": 70.0, + "grid_position_x": 0.0, + "grid_position_y": 0.0, + "maximum_lidar_height_thres": 0.3, + "minimum_lidar_height_thres": -2.2, + "expand_polygon_size": 1.0, + "size_of_expansion_kernel": 9, + }, + vehicle_info_param, + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="freespace_planner", + plugin="freespace_planner::FreespacePlannerNode", + name="freespace_planner", + remappings=[ + ("~/input/route", "/planning/mission_planning/route"), + ("~/input/occupancy_grid", "costmap_generator/occupancy_grid"), + ("~/input/scenario", "/planning/scenario_planning/scenario"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/output/trajectory", "/planning/scenario_planning/parking/trajectory"), + ("is_completed", "/planning/scenario_planning/parking/is_completed"), + ], + parameters=[ + freespace_planner_param, + vehicle_info_param, + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ], + ) + + group = GroupAction( + [ + PushRosNamespace("parking"), + container, + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + # component + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/parking.launch.xml new file mode 100644 index 0000000..6e3a183 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml new file mode 100644 index 0000000..1bcc85f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/package.xml new file mode 100644 index 0000000..7033ab6 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/package.xml @@ -0,0 +1,79 @@ + + + + tier4_planning_launch + 0.1.0 + The tier4_planning_launch package + + + Zulfaqar Azmi + + Kosuke Takeuchi + + Kosuke Takeuchi + + Fumiya Watanabe + + Takamasa Horibe + Satoshi Ota + + Yutaka Shimizu + + Takayuki Murooka + + + + Mamoru Sobue + + Satoshi Ota + + Kyoichi Sugahara + + Taiki Tanaka + + Kosuke Takeuchi + + Satoshi Ota + + Mamoru Sobue + + Yutaka Shimizu + + Kosuke Takeuchi + + Tomohito Ando + Makoto Kurihara + + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + behavior_path_planner + behavior_velocity_planner + costmap_generator + external_cmd_selector + external_velocity_limit_selector + freespace_planner + mission_planner + motion_velocity_smoother + obstacle_avoidance_planner + obstacle_cruise_planner + obstacle_stop_planner + planning_error_monitor + scenario_selector + surround_obstacle_checker + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/planning_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/planning_launch.drawio.svg new file mode 100644 index 0000000..5adb979 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_planning_launch/planning_launch.drawio.svg @@ -0,0 +1,318 @@ + + + + + + + +
+
+
+ planning.launch.xml +
+
+
+ + package: tier4_planning_launch + +
+
+
+
+
+ + planning.launch.xml... + +
+
+ + + + +
+
+
+ mission_planning.launch.py +
+
+
+ package: tier4_planning_launch +
+
+
+
+
+ + mission_planning.launch.py... + +
+
+ + + + + + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+
+ + launch name... + +
+
+ + + + +
+
+
+ ex: +
+
+
+
+ + ex: + +
+
+ + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+
+ + node name... + +
+
+ + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+
+ + other name... + +
+
+ + + + +
+
+
+ scenario_planning.launch.xml +
+
+
+ package: tier4_planning_launch +
+
+
+
+
+ + scenario_planning.launch.xml... + +
+
+ + + + +
+
+
+ mission_planner.launch.xml +
+
+
+ package: mission_planner +
+
+
+
+
+ + mission_planner.launch.xml... + +
+
+ + + + +
+
+
+ goal_pose_visualizer.launch.xml +
+
+
+ package: mission_planner +
+
+
+
+
+ + goal_pose_visualizer.launch.xml... + +
+
+ + + + + + + + +
+
+
+ scenario_selector.launch.xml +
+
+
+ package: scenario_selector +
+
+
+
+
+ + scenario_selector.launch.xml... + +
+
+ + + + + + +
+
+
+ motion_velocity_optimizer.launch.xml +
+
+
+ package: motion_velocity_optimizer +
+
+
+
+
+ + motion_velocity_optimizer.launch.xml... + +
+
+ + + + + + +
+
+
+ lane_driving.launch.xml +
+
+
+ package: tier4_planning_launch +
+
+
+
+
+ + lane_driving.launch.xml... + +
+
+ + + + +
+
+
+ parking.launch.py +
+
+
+ package: tier4_planning_launch +
+
+
+
+
+ + parking.launch.py... + +
+
+ + + + +
+ + + + + Viewer does not support full SVG 1.1 + + + +
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/CMakeLists.txt new file mode 100644 index 0000000..98033bc --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_sensing_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/README.md new file mode 100644 index 0000000..b07582b --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/README.md @@ -0,0 +1,47 @@ +# tier4_sensing_launch + +## Structure + +![tier4_sensing_launch](./sensing_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `sensing.launch.xml`. + +```xml + + + + + + +``` + +## Launch Directory Structure + +This package finds sensor settings of specified sensor model in `launch`. + +```bash +launch/ +├── aip_x1 # Sensor model name +│ ├── camera.launch.xml # Camera +│ ├── gnss.launch.xml # GNSS +│ ├── imu.launch.xml # IMU +│ ├── lidar.launch.xml # LiDAR +│ └── pointcloud_preprocessor.launch.py # for preprocessing pointcloud +... +``` + +## Notes + +This package finds settings with variables. + +ex.) + +```xml + +``` diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/launch/sensing.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/launch/sensing.launch.xml new file mode 100644 index 0000000..a2dfb23 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/launch/sensing.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/package.xml new file mode 100644 index 0000000..c35082a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/package.xml @@ -0,0 +1,22 @@ + + + + tier4_sensing_launch + 0.1.0 + The tier4_sensing_launch package + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/sensing_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/sensing_launch.drawio.svg new file mode 100644 index 0000000..47ff4ac --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_sensing_launch/sensing_launch.drawio.svg @@ -0,0 +1,228 @@ + + + + + + + +
+
+
+ sensing.launch.xml +
+
+
+ + package: tier4_sensing_launch + +
+
+
+
+
+ + sensing.launch.xml... + +
+
+ + + + +
+
+
+ lidar.launch.xml +
+
+
+ package: tier4_sensing_launch +
+
+
+
+
+ + lidar.launch.xml... + +
+
+ + + + + + + + +
+
+
+ camera.launch.xml +
+
+
+ + package: tier4_sensing_launch + +
+
+
+
+
+ + camera.launch.xml... + +
+
+ + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+
+ + launch name... + +
+
+ + + + +
+
+
+ ex: +
+
+
+
+ + ex: + +
+
+ + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+
+ + node name... + +
+
+ + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+
+ + other name... + +
+
+ + + + +
+
+
+ imu.launch.xml +
+
+
+ + package: tier4_sensing_launch + +
+
+
+
+
+ + imu.launch.xml... + +
+
+ + + + +
+
+
+ imu.launch.xml +
+
+
+ + package: tier4_sensing_launch + +
+
+
+
+
+ + imu.launch.xml... + +
+
+ + + + +
+ + + + + Viewer does not support full SVG 1.1 + + + +
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/CMakeLists.txt new file mode 100644 index 0000000..6817e31 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_simulator_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/README.md new file mode 100644 index 0000000..5fbefea --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/README.md @@ -0,0 +1,20 @@ +# tier4_simulator_launch + +## Structure + +![tier4_simulator_launch](./simulator_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +```xml + + + + +``` + +The simulator model used in simple_planning_simulator is loaded from "config/simulator_model.param.yaml" in the "`VEHICLE_MODEL`\_description" package. diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/config/fault_injection.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/config/fault_injection.param.yaml new file mode 100644 index 0000000..1a57b85 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/config/fault_injection.param.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + event_diag_list: + vehicle_is_out_of_lane: "lane_departure" + trajectory_deviation_is_high: "trajectory_deviation" + localization_matching_score_is_low: "ndt_scan_matcher" + localization_accuracy_is_low: "localization_accuracy" + map_version_is_different: "map_version" + trajectory_is_invalid: "trajectory_point_validation" + cpu_temperature_is_high: "CPU Temperature" + cpu_usage_is_high: "CPU Usage" + cpu_is_in_thermal_throttling: "CPU Thermal Throttling" + storage_temperature_is_high: "HDD Temperature" + storage_usage_is_high: "HDD Usage" + network_usage_is_high: "Network Usage" + clock_error_is_large: "NTP Offset" + gpu_temperature_is_high: "GPU Temperature" + gpu_usage_is_high: "GPU Usage" + gpu_memory_usage_is_high: "GPU Memory Usage" + gpu_is_in_thermal_throttling: "GPU Thermal Throttling" + driving_recorder_storage_error: "driving_recorder" + debug_data_logger_storage_error: "bagpacker" + emergency_stop_operation: "emergency_stop_operation" + vehicle_error_occurred: "vehicle_errors" + vehicle_ecu_connection_is_lost: "can_bus_connection" + obstacle_crash_sensor_is_activated: "obstacle_crash" + /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat" + /control/autonomous_driving/node_alive_monitoring: "control_topic_status" + /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat" + /localization/node_alive_monitoring: "localization_topic_status" + /map/node_alive_monitoring: "map_topic_status" + /planning/node_alive_monitoring: "planning_topic_status" + /sensing/lidar/node_alive_monitoring: "lidar_topic_status" + /sensing/imu/node_alive_monitoring: "imu_connection" + /sensing/gnss/node_alive_monitoring: "gnss_connection" + /system/node_alive_monitoring: "system_topic_status" + /vehicle/node_alive_monitoring: "vehicle_topic_status" diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/launch/simulator.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/launch/simulator.launch.xml new file mode 100644 index 0000000..608d98c --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/launch/simulator.launch.xml @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/package.xml new file mode 100644 index 0000000..7964e7c --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/package.xml @@ -0,0 +1,24 @@ + + + + tier4_simulator_launch + 0.1.0 + The tier4_simulator_launch package + Keisuke Shima + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + dummy_perception_publisher + fault_injection + simple_planning_simulator + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/simulator_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/simulator_launch.drawio.svg new file mode 100644 index 0000000..dee6098 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_simulator_launch/simulator_launch.drawio.svg @@ -0,0 +1,4 @@ + + + +
simulator.launch.xml

package: tier4_simulator_launch
simulator.launch.xml...
fault_injection.launch.xml

package: fault_injection
fault_injection.launch.xm...
simple_planning_simulator.launch.xml

package: simple_planning_simulator
simple_planning_simulator.launch.xml...
True
True
$(var vehicle_simulation)
$(var vehicle_simulation)
launch name

package: package name
launch name...
ex:
ex:
node name

package: package name
node name...
$(var scenario_simulation)
$(var scenario_simulation)
False
False
other name

package: package name
other name...
True
True
$(var scenario_simulation)
$(var scenario_simulation)
False
False
dummy_perception_publisher.launch.xml

package: dummy_perception_publisher
dummy_perception_publisher.launch.xml...
Viewer does not support full SVG 1.1
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/CMakeLists.txt new file mode 100644 index 0000000..8917094 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_system_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/README.md new file mode 100644 index 0000000..252701d --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/README.md @@ -0,0 +1,20 @@ +# tier4_system_launch + +## Structure + +![tier4_system_launch](./system_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +```xml + + + + +``` + +The sensing configuration parameters used in system_error_monitor are loaded from "config/diagnostic_aggregator/sensor_kit.param.yaml" in the "`SENSOR_MODEL`\_description" package. diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml new file mode 100644 index 0000000..7a16c35 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml @@ -0,0 +1,155 @@ +# AD Service State Monitor Parameters +/**: + ros__parameters: + # modules_names: string array + module_names: [ + "map", + "sensing", + "perception", + "localization", + "planning", + "control", + "vehicle", + "system" + ] + + # Topic Config + ## names: string array + ## configs: top level + ### names used to declare parameter group + #### module: string + #### timeout[s]: double (0: none) + #### warn_rate[hz]: double (0: none) + topic_configs: + names: [ + "/map/vector_map", + "/map/pointcloud_map", + "/perception/obstacle_segmentation/pointcloud", + "/perception/object_recognition/objects", + "/initialpose3d", + "/localization/pose_twist_fusion_filter/pose", + "/planning/mission_planning/route", + "/planning/scenario_planning/trajectory", + "/control/trajectory_follower/control_cmd", + "/control/command/control_cmd", + "/vehicle/status/velocity_status", + "/vehicle/status/steering_status", + "/system/emergency/control_cmd" + ] + + configs: + /map/vector_map: + module: "map" + timeout: 0.0 + warn_rate: 0.0 + type: "autoware_auto_mapping_msgs/msg/HADMapBin" + transient_local: True + + /map/pointcloud_map: + module: "map" + timeout: 0.0 + warn_rate: 0.0 + type: "sensor_msgs/msg/PointCloud2" + transient_local: True + + /perception/obstacle_segmentation/pointcloud: + module: "sensing" + timeout: 1.0 + warn_rate: 5.0 + type: "sensor_msgs/msg/PointCloud2" + best_effort: True + + /initialpose3d: + module: "localization" + timeout: 0.0 + warn_rate: 0.0 + type: "geometry_msgs/msg/PoseWithCovarianceStamped" + + /localization/pose_twist_fusion_filter/pose: + module: "localization" + timeout: 1.0 + warn_rate: 5.0 + type: "geometry_msgs/msg/PoseStamped" + + # Can be both with feature array or without + # In prediction.launch.xml this is set to without + /perception/object_recognition/objects: + module: "perception" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_perception_msgs/msg/PredictedObjects" + # This topic could have two different types depending on the launch flags used. + # The implementation of subscriptions in ROS2 does not allow for multiple types + # to be defined for a topic. By default this is set to not use have features. + # type: ["tier4_perception_msgs/msg/DynamicObjectArray", "tier4_perception_msgs/msg/DynamicObjectWithFeatureArray"] + + /planning/mission_planning/route: + module: "planning" + timeout: 0.0 + warn_rate: 0.0 + type: "autoware_auto_planning_msgs/msg/HADMapRoute" + transient_local: True + + /planning/scenario_planning/trajectory: + module: "planning" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_planning_msgs/msg/Trajectory" + + /control/trajectory_follower/control_cmd: + module: "control" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_control_msgs/msg/AckermannControlCommand" + + /control/command/control_cmd: + module: "control" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_control_msgs/msg/AckermannControlCommand" + + /vehicle/status/velocity_status: + module: "vehicle" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_vehicle_msgs/msg/VelocityReport" + + /vehicle/status/steering_status: + module: "vehicle" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_vehicle_msgs/msg/SteeringReport" + + /system/emergency/control_cmd: + module: "system" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_control_msgs/msg/AckermannControlCommand" + + # Param Config + ## names: string array + ## configs: top level + ### names used to declare parameter group + #### module: string + # param_configs: + # names: ["/vehicle_info"] + # configs: + # /vehicle_info: + # module: "vehicle" + + # TF Config + ## names: string array + ## configs: top level + ### names used to declare parameter group + #### module: string + #### from: string + #### to: string + #### timeout[s]: double (0: none) + tf_configs: + names: ["map_to_base_link"] + configs: + map_to_base_link: + module: "localization" + from: "map" + to: "base_link" + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml new file mode 100644 index 0000000..51fbe99 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml @@ -0,0 +1,126 @@ +# AD Service State Monitor: Planning Simulator Parameters +/**: + ros__parameters: + # modules_names: string array + module_names: [ + "map", + "sensing", + "perception", + "localization", + "planning", + "control", + "vehicle", + "system" + ] + +# Topic Config + ## names: string array + ## configs: top level + ### names used to declare parameter group + #### module: string + #### timeout[s]: double (0: none) + #### warn_rate[hz]: double (0: none) + topic_configs: + names: [ + "/map/vector_map", + "/perception/object_recognition/objects", + "/initialpose3d", + "/planning/mission_planning/route", + "/planning/scenario_planning/trajectory", + "/control/trajectory_follower/control_cmd", + "/control/command/control_cmd", + "/vehicle/status/velocity_status", + "/vehicle/status/steering_status", + "/system/emergency/control_cmd" + ] + + configs: + /map/vector_map: + module: "map" + timeout: 0.0 + warn_rate: 0.0 + type: "autoware_auto_mapping_msgs/msg/HADMapBin" + transient_local: True + + /perception/object_recognition/objects: + module: "perception" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_perception_msgs/msg/PredictedObjects" + + /initialpose3d: + module: "localization" + timeout: 0.0 + warn_rate: 0.0 + type: "geometry_msgs/msg/PoseWithCovarianceStamped" + + /planning/mission_planning/route: + module: "planning" + timeout: 0.0 + warn_rate: 0.0 + type: "autoware_auto_planning_msgs/msg/HADMapRoute" + transient_local: True + + /planning/scenario_planning/trajectory: + module: "planning" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_planning_msgs/msg/Trajectory" + + /control/trajectory_follower/control_cmd: + module: "control" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_control_msgs/msg/AckermannControlCommand" + + /control/command/control_cmd: + module: "control" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_control_msgs/msg/AckermannControlCommand" + + /vehicle/status/velocity_status: + module: "vehicle" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_vehicle_msgs/msg/VelocityReport" + + /vehicle/status/steering_status: + module: "vehicle" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_vehicle_msgs/msg/SteeringReport" + + /system/emergency/control_cmd: + module: "system" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_control_msgs/msg/AckermannControlCommand" + + # Param Config + ## names: string array + ## configs: top level + ### names used to declare parameter group + #### module: string + # param_configs: + # names: ["/vehicle_info"] + # configs: + # /vehicle_info: + # module: "vehicle" + + # TF Config + ## names: string array + ## configs: top level + ### names used to declare parameter group + #### module: string + #### from: string + #### to: string + #### timeout[s]: double (0: none) + tf_configs: + names: ["map_to_base_link"] + configs: + map_to_base_link: + module: "localization" + from: "map" + to: "base_link" + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/diagnostic_aggregator/vehicle.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/diagnostic_aggregator/vehicle.param.yaml new file mode 100644 index 0000000..e96e3b3 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/diagnostic_aggregator/vehicle.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + vehicle: + type: diagnostic_aggregator/AnalyzerGroup + path: vehicle + analyzers: + vehicle_errors: + type: diagnostic_aggregator/GenericAnalyzer + path: vehicle_errors + contains: [": vehicle_errors"] + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml new file mode 100644 index 0000000..912e484 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml @@ -0,0 +1,13 @@ +# Default configuration for emergency handler +--- +/**: + ros__parameters: + update_rate: 10 + timeout_hazard_status: 0.5 + timeout_takeover_request: 10.0 + use_takeover_request: false + use_parking_after_stopped: false + + # setting whether to turn hazard lamp on for each situation + turning_hazard_on: + emergency: true diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/_empty.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/_empty.param.yaml new file mode 100644 index 0000000..e69de29 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/control.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/control.param.yaml new file mode 100644 index 0000000..0a255b7 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/control.param.yaml @@ -0,0 +1,95 @@ +/**: + ros__parameters: + control: + type: diagnostic_aggregator/AnalyzerGroup + path: control + analyzers: + control_command_gate: + type: diagnostic_aggregator/AnalyzerGroup + path: control_command_gate + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + heartbeat: + type: diagnostic_aggregator/GenericAnalyzer + path: heartbeat + contains: ["vehicle_cmd_gate: heartbeat"] + timeout: 1.0 + + autonomous_driving: + type: diagnostic_aggregator/AnalyzerGroup + path: autonomous_driving + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": control_topic_status"] + timeout: 1.0 + + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + lane_departure: + type: diagnostic_aggregator/GenericAnalyzer + path: lane_departure + contains: [": lane_departure"] + timeout: 1.0 + + trajectory_deviation: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_deviation + contains: [": trajectory_deviation"] + timeout: 1.0 + + external_control: + type: diagnostic_aggregator/AnalyzerGroup + path: external_control + analyzers: + external_command_selector: + type: diagnostic_aggregator/AnalyzerGroup + path: external_command_selector + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + heartbeat: + type: diagnostic_aggregator/GenericAnalyzer + path: heartbeat + contains: ["external_cmd_selector: heartbeat"] + timeout: 1.0 + + local_external_control: + type: diagnostic_aggregator/AnalyzerGroup + path: local_external_control + analyzers: + device_connection: + type: diagnostic_aggregator/AnalyzerGroup + path: device_connection + analyzers: + joy_controller_connection: + type: diagnostic_aggregator/GenericAnalyzer + path: joy_controller_connection + contains: [": joy_controller_connection"] + timeout: 1.0 + + remote_external_control: + type: diagnostic_aggregator/AnalyzerGroup + path: remote_external_control + analyzers: + network_connection: + type: diagnostic_aggregator/AnalyzerGroup + path: network_connection + analyzers: + remote_control_topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: remote_control_topic_status + contains: [": remote_control_topic_status"] + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/localization.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/localization.param.yaml new file mode 100644 index 0000000..2d0b2b0 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/localization.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + localization: + type: diagnostic_aggregator/AnalyzerGroup + path: localization + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": localization_topic_status"] + timeout: 1.0 + + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + matching_score: + type: diagnostic_aggregator/GenericAnalyzer + path: matching_score + contains: ["ndt_scan_matcher"] + timeout: 1.0 + + localization_accuracy: + type: diagnostic_aggregator/GenericAnalyzer + path: localization_accuracy + contains: [": localization_accuracy"] + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/map.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/map.param.yaml new file mode 100644 index 0000000..fa218ca --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/map.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + map: + type: diagnostic_aggregator/AnalyzerGroup + path: map + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": map_topic_status"] + timeout: 1.0 + + # TODO(Tier IV): Support this diagnostics + # route_validation: + # type: diagnostic_aggregator/GenericAnalyzer + # path: route_validation + # contains: [": route_validation"] + # timeout: 0.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/perception.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/perception.param.yaml new file mode 100644 index 0000000..1f897de --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/perception.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + perception: + type: diagnostic_aggregator/AnalyzerGroup + path: perception + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": perception_topic_status"] + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/planning.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/planning.param.yaml new file mode 100644 index 0000000..151c624 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/planning.param.yaml @@ -0,0 +1,47 @@ +/**: + ros__parameters: + planning: + type: diagnostic_aggregator/AnalyzerGroup + path: planning + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": planning_topic_status"] + timeout: 1.0 + + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + trajectory_validation: + type: diagnostic_aggregator/AnalyzerGroup + path: trajectory_validation + analyzers: + trajectory_finite_value: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_point_validation + contains: [": trajectory_point_validation"] + timeout: 1.0 + + trajectory_interval: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_interval_validation + contains: [": trajectory_interval_validation"] + timeout: 1.0 + + trajectory_curvature: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_curvature_validation + contains: [": trajectory_curvature_validation"] + timeout: 1.0 + + trajectory_sharp_angle: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_relative_angle_validation + contains: [": trajectory_relative_angle_validation"] + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/sensing.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/sensing.param.yaml new file mode 100644 index 0000000..887e671 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/sensing.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + sensing: + type: diagnostic_aggregator/AnalyzerGroup + path: sensing + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": sensing_topic_status"] + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/system.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/system.param.yaml new file mode 100644 index 0000000..af6b9ab --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/system.param.yaml @@ -0,0 +1,173 @@ +/**: + ros__parameters: + system: + type: diagnostic_aggregator/AnalyzerGroup + path: system + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": system_topic_status"] + timeout: 1.0 + + emergency_stop_operation: + type: diagnostic_aggregator/GenericAnalyzer + path: emergency_stop_operation + contains: [": emergency_stop_operation"] + timeout: 1.0 + + resource_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: resource_monitoring + analyzers: + clock: + type: diagnostic_aggregator/AnalyzerGroup + path: clock + analyzers: + clock_offset: + type: diagnostic_aggregator/GenericAnalyzer + path: clock_offset + contains: [": NTP Offset"] + timeout: 10.0 + + cpu: + type: diagnostic_aggregator/AnalyzerGroup + path: cpu + analyzers: + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": CPU Temperature"] + timeout: 3.0 + + usage: + type: diagnostic_aggregator/GenericAnalyzer + path: usage + contains: [": CPU Usage"] + timeout: 3.0 + + thermal_throttling: + type: diagnostic_aggregator/GenericAnalyzer + path: thermal_throttling + contains: [": CPU Thermal Throttling"] + timeout: 3.0 + + frequency: + type: diagnostic_aggregator/GenericAnalyzer + path: frequency + contains: [": CPU Frequency"] + timeout: 3.0 + + load_average: + type: diagnostic_aggregator/GenericAnalyzer + path: load_average + contains: [": CPU Load Average"] + timeout: 3.0 + + gpu: + type: diagnostic_aggregator/AnalyzerGroup + path: gpu + analyzers: + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": GPU Temperature"] + timeout: 3.0 + + usage: + type: diagnostic_aggregator/GenericAnalyzer + path: gpu_usage + contains: [": GPU Usage"] + timeout: 3.0 + + memory_usage: + type: diagnostic_aggregator/GenericAnalyzer + path: memory_usage + contains: [": GPU Memory Usage"] + timeout: 3.0 + + thermal_throttling: + type: diagnostic_aggregator/GenericAnalyzer + path: thermal_throttling + contains: [": GPU Thermal Throttling"] + timeout: 3.0 + + memory: + type: diagnostic_aggregator/AnalyzerGroup + path: memory + analyzers: + usage: + type: diagnostic_aggregator/GenericAnalyzer + path: usage + contains: [": Memory Usage"] + timeout: 3.0 + + network: + type: diagnostic_aggregator/AnalyzerGroup + path: network + analyzers: + network_usage: + type: diagnostic_aggregator/GenericAnalyzer + path: network_usage + contains: [": Network Usage"] + timeout: 3.0 + + network_traffic: + type: diagnostic_aggregator/GenericAnalyzer + path: network_traffic + contains: [": Network Traffic"] + timeout: 3.0 + + storage: + type: diagnostic_aggregator/AnalyzerGroup + path: storage + analyzers: + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": HDD Temperature"] + timeout: 3.0 + + usage: + type: diagnostic_aggregator/GenericAnalyzer + path: usage + contains: [": HDD Usage"] + timeout: 3.0 + + power_on_hours: + type: diagnostic_aggregator/GenericAnalyzer + path: power_on_hours + contains: [": HDD PowerOnHours"] + timeout: 3.0 + + total_data_written: + type: diagnostic_aggregator/GenericAnalyzer + path: total_data_written + contains: [": HDD TotalDataWritten"] + timeout: 3.0 + + process: + type: diagnostic_aggregator/AnalyzerGroup + path: process + analyzers: + high_load: + type: diagnostic_aggregator/GenericAnalyzer + path: high_load + contains: [": High-load"] + timeout: 3.0 + + high_mem: + type: diagnostic_aggregator/GenericAnalyzer + path: high_mem + contains: [": High-mem"] + timeout: 3.0 + + tasks_summary: + type: diagnostic_aggregator/GenericAnalyzer + path: tasks_summary + contains: [": Tasks Summary"] + timeout: 3.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml new file mode 100644 index 0000000..45ede1a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + vehicle: + type: diagnostic_aggregator/AnalyzerGroup + path: vehicle + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + # TODO(Tier IV): Consider splitting sensor input and control command output + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": vehicle_topic_status"] + timeout: 1.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml new file mode 100644 index 0000000..b9a5a4f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml @@ -0,0 +1,50 @@ +# Description: +# name: diag name +# sf_at: diag level where it becomes Safe Fault +# lf_at: diag level where it becomes Latent Fault +# spf_at: diag level where it becomes Single Point Fault +# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. +# +# Note: +# empty-value for sf_at, lf_at and spf_at is "none" +# default values are: +# sf_at: "none" +# lf_at: "warn" +# spf_at: "error" +# auto_recovery: "true" +--- +/**: + ros__parameters: + required_modules: + autonomous_driving: + /autoware/control/autonomous_driving/node_alive_monitoring: default + /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + /autoware/control/control_command_gate/node_alive_monitoring: default + + /autoware/localization/node_alive_monitoring: default + /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/localization/performance_monitoring/localization_accuracy: default + + /autoware/map/node_alive_monitoring: default + + /autoware/perception/node_alive_monitoring: default + + /autoware/planning/node_alive_monitoring: default + /autoware/planning/performance_monitoring/trajectory_validation: default + + /autoware/sensing/node_alive_monitoring: default + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } + + /autoware/vehicle/node_alive_monitoring: default + + external_control: + /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/external_control/external_command_selector/node_alive_monitoring: default + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + + /autoware/vehicle/node_alive_monitoring: default diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml new file mode 100644 index 0000000..ce081b7 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml @@ -0,0 +1,50 @@ +# Description: +# name: diag name +# sf_at: diag level where it becomes Safe Fault +# lf_at: diag level where it becomes Latent Fault +# spf_at: diag level where it becomes Single Point Fault +# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. +# +# Note: +# empty-value for sf_at, lf_at and spf_at is "none" +# default values are: +# sf_at: "none" +# lf_at: "warn" +# spf_at: "error" +# auto_recovery: "true" +--- +/**: + ros__parameters: + required_modules: + autonomous_driving: + /autoware/control/autonomous_driving/node_alive_monitoring: default + /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + /autoware/control/control_command_gate/node_alive_monitoring: default + + /autoware/localization/node_alive_monitoring: default + # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } + # /autoware/localization/performance_monitoring/localization_accuracy: default + + /autoware/map/node_alive_monitoring: default + + /autoware/perception/node_alive_monitoring: default + + /autoware/planning/node_alive_monitoring: default + /autoware/planning/performance_monitoring/trajectory_validation: default + + /autoware/sensing/node_alive_monitoring: default + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } + + /autoware/vehicle/node_alive_monitoring: default + + external_control: + /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/external_control/external_command_selector/node_alive_monitoring: default + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + + /autoware/vehicle/node_alive_monitoring: default diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml new file mode 100644 index 0000000..da4d74e --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + usage_warn: 0.96 + usage_error: 0.96 + usage_warn_count: 1 + usage_error_count: 2 + usage_avg: true + msr_reader_port: 7634 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml new file mode 100644 index 0000000..d96b9f2 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + temp_warn: 90.0 + temp_error: 95.0 + gpu_usage_warn: 0.90 + gpu_usage_error: 1.00 + memory_usage_warn: 0.95 + memory_usage_error: 0.99 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml new file mode 100644 index 0000000..04dd3af --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + hdd_reader_port: 7635 + num_disks: 1 + disks: # Until multi type lists are allowed, name N the disks as disk0...disk{N-1} + disk0: + name: /dev/sda3 + temp_attribute_id: 0xC2 + temp_warn: 55.0 + temp_error: 70.0 + power_on_hours_attribute_id: 0x09 + power_on_hours_warn: 3000000 + total_data_written_attribute_id: 0xF1 + total_data_written_warn: 4915200 # =150TB (1unit=32MB) + total_data_written_safety_factor: 0.05 + recovered_error_attribute_id: 0xC3 + recovered_error_warn: 1 + free_warn: 5120 # MB(8hour) + free_error: 100 # MB(last 1 minute) + read_data_rate_warn: 360.0 # MB/s + write_data_rate_warn: 103.5 # MB/s + read_iops_warn: 63360.0 # IOPS + write_iops_warn: 24120.0 # IOPS diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml new file mode 100644 index 0000000..f2da397 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + available_size: 1024 # MB diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/net_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/net_monitor.param.yaml new file mode 100644 index 0000000..d72b8d1 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/net_monitor.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + devices: ["*"] + traffic_reader_port: 7636 + monitor_program: "greengrass" + crc_error_check_duration: 1 + crc_error_count_threshold: 1 + reassembles_failed_check_duration: 1 + reassembles_failed_check_count: 1 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml new file mode 100644 index 0000000..db54f70 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + server: ntp.nict.jp + offset_warn: 0.1 + offset_error: 5.0 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/process_monitor.param.yaml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/process_monitor.param.yaml new file mode 100644 index 0000000..3d6d82f --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/config/system_monitor/process_monitor.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + num_of_procs: 5 diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/launch/system.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/launch/system.launch.xml new file mode 100644 index 0000000..5e7153a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/launch/system.launch.xml @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/package.xml new file mode 100644 index 0000000..f496ac8 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/package.xml @@ -0,0 +1,26 @@ + + + + tier4_system_launch + 0.1.0 + The tier4_system_launch package + Kenji Miyake + Fumihito Ito + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + ad_service_state_monitor + emergency_handler + system_error_monitor + system_monitor + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/system_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/system_launch.drawio.svg new file mode 100644 index 0000000..30a2bc0 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_system_launch/system_launch.drawio.svg @@ -0,0 +1,408 @@ + + + + + + + +
+
+
+ system.launch.xml +
+
+
+ + package: tier4_system_launch + +
+
+
+
+
+ + system.launch.xml... + +
+
+ + + + +
+
+
+ system_monitor.launch.py +
+
+
+ package: system_monitor +
+
+
+
+
+ + system_monitor.launch.py... + +
+
+ + + + + + + + + +
+
+
+ online +
+
+
+
+ + online + +
+
+ + + + + +
+
+
+ planning_simulation +
+
+
+
+ + planning_simulation + +
+
+ + + + +
+
+
+ $(var run_mode) +
+
+
+
+ + $(var run_mode) + +
+
+ + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+
+ + launch name... + +
+
+ + + + +
+
+
+ ex: +
+
+
+
+ + ex: + +
+
+ + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+
+ + node name... + +
+
+ + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+
+ + other name... + +
+
+ + + + +
+
+
+ ad_service_state_monitor.launch.xml +
+
+
+ package: ad_service_state_monitor +
+
+
+
+
+ args: config_file = + + ad_service_state_monitor.param.yaml + +
+
+
+
+
+ + ad_service_state_monitor.launch.xml... + +
+
+ + + + +
+
+
+ ad_service_state_monitor.launch.xml +
+
+
+ package: ad_service_state_monitor +
+
+
+
+
+ args: config_file = + + ad_service_state_monitor.planning_simulation.param.yaml + +
+
+
+
+
+ + ad_service_state_monitor.launch.xml... + +
+
+ + + + + + + +
+
+
+ online +
+
+
+
+ + online + +
+
+ + + + + +
+
+
+ planning_simulation +
+
+
+
+ + planning_simulation + +
+
+ + + + +
+
+
+ $(var run_mode) +
+
+
+
+ + $(var run_mode) + +
+
+ + + + +
+
+
+ system_error_monitor.launch.xml +
+
+
+ package: system_error_monitor +
+
+
+
+
+ args: config_file = + + system_error_monitor.param.yaml + +
+
+
+
+
+ + system_error_monitor.launch.xml... + +
+
+ + + + +
+
+
+ system_error_monitor.launch.xml +
+
+
+ package: system_error_monitor +
+
+
+
+
+ args: config_file = + + system_error_monitor.planning_simulation.param.yaml + +
+
+
+
+
+ + system_error_monitor.launch.xml... + +
+
+ + + + +
+
+
+ emergency_handler.launch.xml +
+
+
+ package: emergency_handler +
+
+
+
+
+ + emergency_handler.launch.xml... + +
+
+ + +
+ + + + + Viewer does not support full SVG 1.1 + + + +
\ No newline at end of file diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/CMakeLists.txt b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/CMakeLists.txt new file mode 100644 index 0000000..bde5370 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_vehicle_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE + launch + urdf +) diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/README.md b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/README.md new file mode 100644 index 0000000..3857ce4 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/README.md @@ -0,0 +1,61 @@ +# tier4_vehicle_launch + +## Structure + +![tier4_vehicle_launch](./vehicle_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `vehicle.launch.xml`. + +```xml + + + + + + + +``` + +## Notes + +This package finds some external packages and settings with variables and package names. + +ex.) + +```xml + +``` + +```xml + +``` + +## vehicle.xacro + +### Arguments + +| Name | Type | Description | Default | +| ------------- | ------ | ------------------ | ------- | +| sensor_model | String | sensor model name | "" | +| vehicle_model | String | vehicle model name | "" | + +### Usage + +You can write as follows in `*.launch.xml`. + +```xml + + + + + + + + +``` diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/launch/vehicle.launch.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/launch/vehicle.launch.xml new file mode 100644 index 0000000..436a48a --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/package.xml b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/package.xml new file mode 100644 index 0000000..5013167 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/package.xml @@ -0,0 +1,23 @@ + + + + tier4_vehicle_launch + 0.1.0 + The tier4_vehicle_launch package + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + robot_state_publisher + xacro + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/urdf/vehicle.xacro b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/urdf/vehicle.xacro new file mode 100644 index 0000000..8c3ecf9 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/urdf/vehicle.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/vehicle_launch.drawio.svg b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/vehicle_launch.drawio.svg new file mode 100644 index 0000000..32c17b1 --- /dev/null +++ b/autoware/aichallenge_ws/src/aichallenge_submit/tier4_vehicle_launch/vehicle_launch.drawio.svg @@ -0,0 +1,4 @@ + + + +
vehicle.launch.xml

package: tier4_vehicle_launch
vehicle.launch.xml...
vehicle_description.launch.xml

package: tier4_vehicle_launch
vehicle_description.launch.xml...
vehicle_interface.launch.xml

package: $(var vehicle_model)_description
vehicle_interface.launch.xml...
False
False
$(var simulation)
$(var simulation)
vehicle.xacro

package: tier4_vehicle_launch
  
vehicle.xacro...
vehicle.xacro

package: $(var vehicle_model)_description
vehicle.xacro...
sensors.xacro

package: $(var sensor_model)_description
sensors.xacro...
launch name

package: package name
launch name...
ex:
ex:
node name

package: package name
node name...
robot state_publisher

package: robot_state_publisher
robot state_publisher...
other name

package: package name
other name...
Viewer does not support full SVG 1.1
\ No newline at end of file diff --git a/evaluation/Dockerfile b/evaluation/Dockerfile new file mode 100644 index 0000000..e502405 --- /dev/null +++ b/evaluation/Dockerfile @@ -0,0 +1,32 @@ +FROM ghcr.io/automotiveaichallenge/aichallenge2022-sim/autoware-universe-cuda:3.1 + +ARG NVIDIA_VISIBLE_DEVICES=all +ARG NVIDIA_DRIVER_CAPABILITIES=compute,utility,graphics,display +ARG DEBIAN_FRONTEND=noninteractive + +# Copy map +COPY nishishinjuku_autoware_map /aichallenge/nishishinjuku_autoware_map + +# Copy common ros2 packages +COPY aichallenge_ws /aichallenge/aichallenge_ws + +# Copy user files +COPY aichallenge_submit.tar.gz /aichallenge_submit.tar.gz +RUN cd / \ + && mkdir /aichallenge/aichallenge_ws/src/aichallenge_submit \ + && tar zxf aichallenge_submit.tar.gz -C /aichallenge/aichallenge_ws/src/aichallenge_submit \ + && rm /aichallenge_submit.tar.gz + +# build +RUN apt-get update +RUN bash -c ' \ + source /autoware/install/setup.bash; \ + cd /aichallenge/aichallenge_ws; \ + rosdep update; \ + rosdep install -y -r -i --from-paths src --ignore-src --rosdistro $ROS_DISTRO; \ + colcon build' + +COPY main.bash /main.bash + +ENTRYPOINT [] +CMD ["bash", "/main.bash"] diff --git a/evaluation/build.sh b/evaluation/build.sh new file mode 100644 index 0000000..eb432b1 --- /dev/null +++ b/evaluation/build.sh @@ -0,0 +1,18 @@ +#!/bin/sh + +if [ $# -ne 1 ]; then + echo "Usage: bash build.sh COURSE" + echo "COURSE = 'Challenge' or 'Advanced'" + exit 1 +fi + +mkdir -p output + +mkdir -p aichallenge_ws/src +cp -rd ../autoware/aichallenge_ws/src/aichallenge_eval aichallenge_ws/src/ +cp -rd ../autoware/aichallenge_ws/src/aichallenge_launch aichallenge_ws/src/ +cp -rd ../autoware/aichallenge_ws/src/aichallenge_score_msgs aichallenge_ws/src/ + +cp -rd ../autoware/nishishinjuku_autoware_map . + +docker build -t aichallenge-eval . diff --git a/evaluation/create_submit_tar.sh b/evaluation/create_submit_tar.sh new file mode 100644 index 0000000..0fb1e7a --- /dev/null +++ b/evaluation/create_submit_tar.sh @@ -0,0 +1 @@ +tar zcvf aichallenge_submit.tar.gz ../autoware/aichallenge_ws/src/aichallenge_submit diff --git a/evaluation/main.bash b/evaluation/main.bash new file mode 100644 index 0000000..f409e02 --- /dev/null +++ b/evaluation/main.bash @@ -0,0 +1,12 @@ +#!/bin/bash + +cd /output + +source /aichallenge/aichallenge_ws/install/setup.bash +ros2 launch aichallenge_launch aichallenge.launch.xml & + +# Wait score.js +until [ -f score.json ] +do + sleep 5 +done diff --git a/evaluation/run.sh b/evaluation/run.sh new file mode 100644 index 0000000..6ac46c3 --- /dev/null +++ b/evaluation/run.sh @@ -0,0 +1,15 @@ +#!/bin/sh + +if [ -f "output/score.json" ]; then + rm output/score.json +fi + +mkdir -p output + +docker run \ + -e NVIDIA_VISIBLE_DEVICES=0 -e NVIDIA_DRIVER_CAPABILITIES=utility,graphics,display,compute \ + --env="DISPLAY" \ + -v $PWD/output:/output \ + --net host \ + --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ + --runtime=nvidia -it --rm -- aichallenge-eval