From 26fe6c72a2c698fda31a79c63b25b2011e44bf5c Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 19 Aug 2024 18:41:31 +0900 Subject: [PATCH 01/14] test: temporarily supports source build Signed-off-by: Takagi, Isamu --- aichallenge/workspace/colcon_build.bash | 2 + aichallenge/workspace/depends.repos | 27 ++++ aichallenge/workspace/depends/.gitignore | 2 + .../src/aichallenge_launch/CMakeLists.txt | 7 + .../config/converter.param.yaml | 18 +++ .../src/aichallenge_launch/data/accel_map.csv | 7 + .../src/aichallenge_launch/data/brake_map.csv | 10 ++ .../src/aichallenge_launch/data/steer_map.csv | 14 ++ .../launch/aichallenge.launch.xml | 148 ++++++++++++++++++ .../src/aichallenge_launch/package.xml | 35 +++++ .../workspace/utils/initialize_workspace.bash | 10 ++ 11 files changed, 280 insertions(+) create mode 100755 aichallenge/workspace/colcon_build.bash create mode 100644 aichallenge/workspace/depends.repos create mode 100644 aichallenge/workspace/depends/.gitignore create mode 100644 aichallenge/workspace/src/aichallenge_launch/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_launch/config/converter.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv create mode 100644 aichallenge/workspace/src/aichallenge_launch/data/brake_map.csv create mode 100644 aichallenge/workspace/src/aichallenge_launch/data/steer_map.csv create mode 100644 aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_launch/package.xml create mode 100755 aichallenge/workspace/utils/initialize_workspace.bash diff --git a/aichallenge/workspace/colcon_build.bash b/aichallenge/workspace/colcon_build.bash new file mode 100755 index 00000000..3e5384c9 --- /dev/null +++ b/aichallenge/workspace/colcon_build.bash @@ -0,0 +1,2 @@ +#!/usr/bin/bash +colcon build --symlink-install --packages-up-to aichallenge_launch diff --git a/aichallenge/workspace/depends.repos b/aichallenge/workspace/depends.repos new file mode 100644 index 00000000..75ab280d --- /dev/null +++ b/aichallenge/workspace/depends.repos @@ -0,0 +1,27 @@ +repositories: + # used for autoware cmake and lanelet2 related packages + autoware_common: + type: git + url: https://github.com/autowarefoundation/autoware_common.git + version: awsim-stable + # used for goal pose setter + autoware_adapi_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_adapi_msgs.git + version: 9679b5a7a1f4cfff2fa50b80d2759d3937f2f953 + autoware_auto_msgs: + type: git + url: https://github.com/tier4/autoware_auto_msgs.git + version: f6642370c6f4f42a5dc0b6c1fc4a21396d4dc34c + autoware_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_msgs.git + version: 90ed3f1f3371d75eb30b6e7d2acb88ccec4e8030 + tier4_autoware_msgs: + type: git + url: https://github.com/tier4/tier4_autoware_msgs.git + version: 87febc35abc9310eaec0b6dd3ce2a4845ce515aa + autoware.universe: + type: git + url: https://github.com/autowarefoundation/autoware.universe.git + version: awsim-stable diff --git a/aichallenge/workspace/depends/.gitignore b/aichallenge/workspace/depends/.gitignore new file mode 100644 index 00000000..d6b7ef32 --- /dev/null +++ b/aichallenge/workspace/depends/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/aichallenge/workspace/src/aichallenge_launch/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_launch/CMakeLists.txt new file mode 100644 index 00000000..231f1738 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.14) +project(aichallenge_launch) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE launch config data) diff --git a/aichallenge/workspace/src/aichallenge_launch/config/converter.param.yaml b/aichallenge/workspace/src/aichallenge_launch/config/converter.param.yaml new file mode 100644 index 00000000..3e98b794 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/config/converter.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + use_steer_ff: true + use_steer_fb: true + is_debugging: false + steer_pid: + kp: 150.0 + ki: 15.0 + kd: 0.0 + max: 8.0 + min: -8.0 + max_p: 8.0 + min_p: -8.0 + max_i: 8.0 + min_i: -8.0 + max_d: 0.0 + min_d: 0.0 + invalid_integration_decay: 0.97 diff --git a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv new file mode 100644 index 00000000..32e639ca --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv @@ -0,0 +1,7 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 +0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 +0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 +0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 +0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 diff --git a/aichallenge/workspace/src/aichallenge_launch/data/brake_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/brake_map.csv new file mode 100644 index 00000000..adf2c809 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/data/brake_map.csv @@ -0,0 +1,10 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51 +0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96 +0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633 +0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 +0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 +0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 +0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 +0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/aichallenge/workspace/src/aichallenge_launch/data/steer_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/steer_map.csv new file mode 100644 index 00000000..3da0e0cd --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/data/steer_map.csv @@ -0,0 +1,14 @@ +default,-0.6,-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4,0.5,0.6 +-12,-0.29,-0.3234236132,-0.3468991362,-0.37274847,-0.4288688461,-0.4696573,-0.4781014157,-0.49,-0.51,-0.52,-0.53,-0.54,-0.55 +-10,-0.2,-0.2451308686,-0.2667782734,-0.3090752469,-0.3575200568,-0.3874868999,-0.4041493831,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47 +-8,-0.1,-0.1586739777,-0.2020698818,-0.2317654357,-0.2778998646,-0.3029839842,-0.3188172953,-0.32,-0.33,-0.34,-0.35,-0.36,-0.37 +-6,-0.0404761584,-0.0806574159,-0.1208386734,-0.1559621241,-0.1888907059,-0.210790018,-0.2322336736,-0.24,-0.25,-0.26,-0.27,-0.28,-0.29 +-4,0.01,-0.01428569928,-0.04,-0.06805018099,-0.09718925222,-0.1189509092,-0.1361293637,-0.14,-0.153044463,-0.1688321844,-0.18,-0.2,-0.22 +-2,0.06,0.05,0.03,0.01,-0.006091655083,-0.03546033903,-0.05319488695,-0.06097627388,-0.07576251508,-0.09165178816,-0.1,-0.12,-0.14 +0,0.11,0.09,0.07,0.05,0.03,0.01,-0.0004106386541,-0.01,-0.03,-0.05,-0.07,-0.09,-0.11 +2,0.15,0.13,0.11,0.09,0.07812978496,0.06023545297,0.04386326928,0.02314813566,0.003779338416,-0.01390526686,-0.0324840879,-0.05106290894,-0.06964172998 +4,0.24,0.22,0.2,0.1839795042,0.1649754952,0.1455879759,0.1274837062,0.1020632549,0.0861964856,0.06971503353,0.04541294018,0.01308869356,-0.01923555307 +6,0.33,0.31,0.29,0.274963497,0.2447295892,0.2337353319,0.2126138313,0.1767700907,0.1578757866,0.1440584148,0.1136331403,0.0827443595,0.05185557872 +8,0.43,0.41,0.39,0.3714915121,0.3299578073,0.3156403746,0.2997845963,0.2500495071,0.2339668568,0.2153133621,0.1815127859,0.156071251,0.1306297162 +10,0.5,0.49,0.48,0.4629455165,0.4210353203,0.3977027236,0.3865034288,0.3250957262,0.312237913,0.2885123051,0.2474220915,0.2123900615,0.1773580315 +12,0.56,0.54,0.53,0.5151036525,0.5046070554,0.4897214196,0.4687756354,0.4036994672,0.3812674227,0.3496883008,0.32482655,0.2783538423,0.2318811345 diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml new file mode 100644 index 00000000..e7ef10e4 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml new file mode 100644 index 00000000..e67ecbee --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/package.xml @@ -0,0 +1,35 @@ + + + + aichallenge_launch + 0.0.0 + TODO: Package description + Takagi, Isamu + TODO: License declaration + + ament_cmake_auto + autoware_cmake + aichallenge_submit_launch + tier4_vehicle_launch + global_parameter_loader + goal_pose_setter + map_loader + map_tf_generator + imu_corrector + imu_gnss_poser + gyro_odometer + ekf_localizer + twist2accel + raw_vehicle_cmd_converter + robot_state_publisher + vehicle_velocity_converter + racing_kart_description + racing_kart_sensor_kit_description + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/aichallenge/workspace/utils/initialize_workspace.bash b/aichallenge/workspace/utils/initialize_workspace.bash new file mode 100755 index 00000000..9be731f7 --- /dev/null +++ b/aichallenge/workspace/utils/initialize_workspace.bash @@ -0,0 +1,10 @@ +#!/usr/bin/bash +WORKSPACE=$(readlink -f "$(dirname "$0")/..") +cd "${WORKSPACE}" || exit +vcs import --shallow --input depends.repos depends + +touch depends/autoware.universe/localization/gyro_odometer/COLCON_IGNORE +touch depends/autoware.universe/sensing/imu_corrector/COLCON_IGNORE + +# shellcheck disable=SC2046 +rosdep install --ignore-src --from-paths $(colcon list --paths-only --packages-up-to aichallenge_launch) From 2c6e319b8ac2acc8cce514fe50821db73e5615af Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 19 Aug 2024 20:06:20 +0900 Subject: [PATCH 02/14] add map loader param file Signed-off-by: Takagi, Isamu --- .../aichallenge_launch/config/lanelet2_map_loader.param.yaml | 4 ++++ .../src/aichallenge_launch/launch/aichallenge.launch.xml | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) create mode 100755 aichallenge/workspace/src/aichallenge_launch/config/lanelet2_map_loader.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_launch/config/lanelet2_map_loader.param.yaml new file mode 100755 index 00000000..61c41ee8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/config/lanelet2_map_loader.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + lanelet2_map_projector_type: MGRS # Options: MGRS, UTM + center_line_resolution: 5.0 # [m] diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml index e7ef10e4..d7511ba4 100644 --- a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml @@ -99,7 +99,7 @@ - + @@ -108,7 +108,7 @@ - + From cf112aaa96003ce0b7cf1f0d653d404524c7d9e2 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 20 Aug 2024 20:31:24 +0900 Subject: [PATCH 03/14] feat: add launch arg for sim real (#37) * feat: add launch arg to collaborate with sim/real * chore: update launch arg * feat: run commands from shell * Revert "feat: add launch arg to collaborate with sim/real" This reverts commit 255436fa423584bcd8000e7a8f8ac391d8e4043e. * feat: add launch arg for sim/real (#38) * feat: add launch arg for sim/real * feat: add automatic operation arg for sim * feat: add system arg * fix: launch package * chore: change to sim * fix: echo arg * feat: add converter and map related package * chore: add missing package for param * Update aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --------- Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> * chore: update aichallenge.launch * Update aichallenge/workspace/aichallenge_submit.sh --------- Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- aichallenge/workspace/aichallenge_submit.sh | 7 +++ .../launch/aichallenge.launch.xml | 49 ++++++++++--------- .../launch/aichallenge_submit.launch.xml | 22 +++++++++ 3 files changed, 54 insertions(+), 24 deletions(-) create mode 100644 aichallenge/workspace/aichallenge_submit.sh create mode 100644 aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_submit.launch.xml diff --git a/aichallenge/workspace/aichallenge_submit.sh b/aichallenge/workspace/aichallenge_submit.sh new file mode 100644 index 00000000..4fa784e6 --- /dev/null +++ b/aichallenge/workspace/aichallenge_submit.sh @@ -0,0 +1,7 @@ +#!/usr/bin/bash + +source install/setup.bash + +ros2 launch aichallenge_launch aichallenge_submit.launch.xml \ +use_sim_time:=false \ +launch_vehicle_interface:=true diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml index d7511ba4..2b5c2078 100644 --- a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml @@ -11,6 +11,10 @@ + + + + @@ -99,7 +103,7 @@ - + @@ -108,7 +112,7 @@ - + @@ -123,26 +127,23 @@ - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_submit.launch.xml new file mode 100644 index 00000000..7c029bc4 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_submit.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + From 2b32d682d2e406d9868c3dac510797f9ffc4769a Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Wed, 21 Aug 2024 10:47:47 +0900 Subject: [PATCH 04/14] chore: change stream interval --- .../goal_pose_setter/src/goal_pose_setter_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp index b19c1bde..bc2a50bf 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp @@ -89,7 +89,7 @@ void GoalPosePublisher::on_timer() msg->pose.orientation.w = this->get_parameter("goal.orientation.w").as_double(); goal_publisher_->publish(*msg); - RCLCPP_INFO(this->get_logger(), "Publishing goal pose"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000 /*ms*/, "Publishing goal pose"); } } From d18429b60a5b08adbdb7f131aebc7eec33f374b1 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Thu, 22 Aug 2024 22:05:01 +0900 Subject: [PATCH 05/14] fix: correct imu orientation --- .../imu_corrector/src/imu_corrector_core.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp index edc82f6d..5c84d470 100644 --- a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp @@ -42,6 +42,17 @@ geometry_msgs::msg::Vector3 transformVector3( return vec_stamped_transformed.vector; } +geometry_msgs::msg::Quaternion transformQuaternion( + const geometry_msgs::msg::Quaternion & quat, const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::QuaternionStamped quat_stamped; + quat_stamped.quaternion = quat; + + geometry_msgs::msg::QuaternionStamped quat_stamped_transformed; + tf2::doTransform(quat_stamped, quat_stamped_transformed, transform); + return quat_stamped_transformed.quaternion; +} + namespace imu_corrector { ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options) @@ -103,6 +114,7 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m sensor_msgs::msg::Imu imu_msg_base_link; imu_msg_base_link.header.stamp = imu_msg_ptr->header.stamp; imu_msg_base_link.header.frame_id = output_frame_; + imu_msg_base_link.orientation = transformQuaternion(imu_msg.orientation, *tf_imu2base_ptr); imu_msg_base_link.linear_acceleration = transformVector3(imu_msg.linear_acceleration, *tf_imu2base_ptr); imu_msg_base_link.linear_acceleration_covariance = From fb563af2a41511a3e832f9f0728d8258c0fb4a7d Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Sat, 24 Aug 2024 12:26:22 +0900 Subject: [PATCH 06/14] feat: add aichallenge submit launch arg (#39) * feat(aichallenge_submit_launch): replace from autoware launch to aichallenge submit launch * revert: this line to original * chore: remove unused * chore: move map dir according to launch --------- Co-authored-by: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com> --- .../{ => map}/lanelet2_map_loader.param.yaml | 0 .../mission_planner.param.yaml | 8 ++ .../common/nearest_search.param.yaml | 5 + .../avoidance/avoidance.param.yaml | 116 ++++++++++++++++++ .../drivable_area_expansion.param.yaml | 58 +++++++++ .../lane_change/lane_change.param.yaml | 34 +++++ .../lane_following/lane_following.param.yaml | 4 + .../pull_out/pull_out.param.yaml | 31 +++++ .../pull_over/pull_over.param.yaml | 95 ++++++++++++++ .../side_shift/side_shift.param.yaml | 8 ++ .../launch/reference.launch.xml | 20 +-- 11 files changed, 369 insertions(+), 10 deletions(-) rename aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/{ => map}/lanelet2_map_loader.param.yaml (100%) create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml similarity index 100% rename from aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml rename to aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml new file mode 100644 index 00000000..a801eca3 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + map_frame: map + arrival_check_angle_deg: 45.0 + arrival_check_distance: 1.0 + arrival_check_duration: 1.0 + goal_angle_threshold_deg: 45.0 + enable_correct_goal_pose: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml new file mode 100644 index 00000000..eb670976 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/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/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml new file mode 100644 index 00000000..c14f59f6 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -0,0 +1,116 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + ros__parameters: + avoidance: + resample_interval_for_planning: 0.3 # [m] + resample_interval_for_output: 4.0 # [m] + detection_area_right_expand_dist: 0.0 # [m] + detection_area_left_expand_dist: 1.0 # [m] + drivable_area_right_bound_offset: 0.0 # [m] + drivable_area_left_bound_offset: 0.0 # [m] + object_envelope_buffer: 0.3 # [m] + + # avoidance module common setting + enable_bound_clipping: false + enable_avoidance_over_same_direction: true + enable_avoidance_over_opposite_direction: true + enable_update_path_when_object_is_gone: false + enable_force_avoidance_for_stopped_vehicle: false + enable_safety_check: false + enable_yield_maneuver: false + disable_path_update: false + + # for debug + publish_debug_marker: false + print_debug_info: false + + # 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 + + # For target object filtering + target_filtering: + # filtering moving objects + threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] + # detection range + object_check_force_avoidance_clearance: 30.0 # [m] + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] + # filtering parking objects + threshold_distance_object_is_on_center: 1.0 # [m] + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + # lost object compensation + object_last_seen_threshold: 2.0 + + # For safety check + safety_check: + safety_check_backward_distance: 100.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + safety_check_hysteresis_factor: 2.0 # [-] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] + lateral_passable_safety_buffer: 0.0 # [m] + road_shoulder_safety_margin: 0.3 # [m] + avoidance_execution_lateral_threshold: 0.499 + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + # avoidance distance parameters + longitudinal: + prepare_time: 2.0 # [s] + longitudinal_collision_safety_buffer: 0.0 # [m] + 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] + + # For yield maneuver + yield: + yield_velocity: 2.78 # [m/s] + + # For stop maneuver + stop: + min_distance: 10.0 # [m] + max_distance: 20.0 # [m] + + constraints: + # vehicle slows down under longitudinal constraints + use_constraints_for_decel: false # [-] + + # lateral constraints + lateral: + nominal_lateral_jerk: 0.2 # [m/s3] + max_lateral_jerk: 1.0 # [m/s3] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -2.0 # [m/ss] + max_jerk: 1.0 + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] + + target_velocity_matrix: + col_size: 2 + matrix: + [2.78, 13.9, # velocity [m/s] + 0.50, 1.00] # margin [m] diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml new file mode 100644 index 00000000..902a93ce --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + # Static expansion + avoidance: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_change: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_following: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_out: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_over: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + side_shift: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + + # Dynamic expansion by projecting the ego footprint along the path + dynamic_expansion: + enabled: false + ego: + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the ego footprint + rear: 0.5 # [m] extra length to add to the rear of the ego footprint + left: 0.5 # [m] extra length to add to the left of the ego footprint + right: 0.5 # [m] extra length to add to the rear of the ego footprint + dynamic_objects: + avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the dynamic object footprint + rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + left: 0.5 # [m] extra length to add to the left of the dynamic object footprint + right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + expansion: + method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. + # 'lanelet': add lanelets overlapped by the ego footprints + # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area + max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + avoid_linestring: + types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area + - road_border + distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid + compensate: + enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction + extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml new file mode 100644 index 00000000..295769ac --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -0,0 +1,34 @@ +/**: + ros__parameters: + lane_change: + lane_change_prepare_duration: 4.0 # [s] + lane_changing_safety_check_duration: 8.0 # [s] + + minimum_lane_change_prepare_distance: 2.0 # [m] + minimum_lane_change_length: 16.5 # [m] + backward_length_buffer_for_end_of_lane: 3.0 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] + + lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_acc: 0.5 # [m/s2] + + minimum_lane_change_velocity: 2.78 # [m/s] + prediction_time_resolution: 0.5 # [s] + maximum_deceleration: 1.0 # [m/s2] + lane_change_sampling_num: 3 + + # collision check + enable_collision_check_at_prepare_phase: false + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: false + + abort_delta_time: 3.0 # [s] + abort_max_lateral_jerk: 1000.0 # [m/s3] + + # debug + publish_debug_marker: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml new file mode 100644 index 00000000..b6a9574b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + lane_following: + lane_change_prepare_duration: 2.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml new file mode 100644 index 00000000..68000747 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + pull_out: + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 + collision_check_distance_from_end: 1.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + 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 + divide_pull_out_path: false + 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: 30.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 + ignore_distance_from_lane_end: 15.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml new file mode 100644 index 00000000..ca1eb740 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -0,0 +1,95 @@ +/**: + ros__parameters: + pull_over: + request_length: 100.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: 0.5 + # 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 + max_lateral_offset: 0.5 + lateral_offset_interval: 0.25 + ignore_distance_from_lane_start: 15.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: 1.0 + # freespace parking + enable_freespace_parking: true + freespace_parking: + planning_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.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 + prediction_time_resolution: 0.5 + 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/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml new file mode 100644 index 00000000..79044041 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/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/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml index 5ab2d4b3..4302157b 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml @@ -114,7 +114,7 @@ - + @@ -179,14 +179,14 @@ - - - - - - + + + + + + - + @@ -226,7 +226,7 @@ - + @@ -235,7 +235,7 @@ - + From 531cf90b62d566552f335ad217366c2fa7a162f7 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com> Date: Sun, 25 Aug 2024 14:40:53 +0900 Subject: [PATCH 07/14] feat: add planning and control (#42) * feat(aichallenge_submit_launch): replace from autoware launch to aichallenge submit launch * revert: this line to original * chore: remove unused * chore: move map dir according to launch * feat: add planning and control Signed-off-by: Masahiro Kubota * fix: modify accel map based on rosba Signed-off-by: Masahiro Kubota * fix: modify rosbag Signed-off-by: Masahiro Kubota * chore: delete redundant file Signed-off-by: Masahiro Kubota --------- Signed-off-by: Masahiro Kubota Co-authored-by: taikitanaka3 --- .../src/aichallenge_launch/data/accel_map.csv | 15 +- .../launch/aichallenge.launch.xml | 130 ++++++++++++++++++ .../src/aichallenge_launch/package.xml | 9 ++ 3 files changed, 147 insertions(+), 7 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv index 32e639ca..165e7ee3 100644 --- a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv +++ b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv @@ -1,7 +1,8 @@ -default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 -0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 -0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 -0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 -0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 -0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 -0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 +default,0.0,1.0,2.0,3.0,4.0,5.0 +0.0,0.0,0.0,0.0,0.0,0.0,0.0 +0.1,0.0,0.0,0.0,0.0,0.0,0.0 +0.2,0.0,0.0,0.0,0.0,0.0,0.0 +0.3,0.05,0.05,0.05,0.05,0.05,0.05 +0.4,0.1,0.1,0.1,0.1,0.1,0.1 +0.5,0.175,0.175,0.175,0.175,0.175,0.175 +0.6,0.25,0.25,0.25,0.25,0.25,0.25 diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml index 2b5c2078..24d2722d 100644 --- a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml @@ -92,6 +92,127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -146,4 +267,13 @@ + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml index e67ecbee..a66b3510 100644 --- a/aichallenge/workspace/src/aichallenge_launch/package.xml +++ b/aichallenge/workspace/src/aichallenge_launch/package.xml @@ -26,6 +26,15 @@ racing_kart_description racing_kart_sensor_kit_description + dummy_perception_publisher + mission_planner + behavior_path_planner + path_to_trajectory + default_ad_api + ad_api_adaptors + simple_pure_pursuit + + ament_lint_auto ament_lint_common From 7caa17c1efaf80065d6d5099be822ddb889e56fd Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 27 Aug 2024 16:50:16 +0900 Subject: [PATCH 08/14] test: source build update (#46) * feat: add source build package and launch * feat: add source build command * fix!: sensor kit calib for real vehicle * revert: script install and parallel worker * chore: comment out object marker * fix: update sensor kit calib --- aichallenge/workspace/aichallenge_launch.sh | 5 ++++ aichallenge/workspace/aichallenge_submit.sh | 7 ----- aichallenge/workspace/colcon_build.bash | 2 +- .../launch/aichallenge.launch.xml | 4 +-- .../launch/aichallenge_system.launch.xml | 28 +++++++++++++++++++ .../src/aichallenge_launch/package.xml | 1 + .../config/sensor_kit_calibration.yaml | 10 +++---- 7 files changed, 42 insertions(+), 15 deletions(-) create mode 100644 aichallenge/workspace/aichallenge_launch.sh delete mode 100644 aichallenge/workspace/aichallenge_submit.sh create mode 100644 aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml diff --git a/aichallenge/workspace/aichallenge_launch.sh b/aichallenge/workspace/aichallenge_launch.sh new file mode 100644 index 00000000..9c8fa071 --- /dev/null +++ b/aichallenge/workspace/aichallenge_launch.sh @@ -0,0 +1,5 @@ +#!/usr/bin/bash + +source install/setup.bash + +ros2 launch aichallenge_launch aichallenge_system.launch.xml diff --git a/aichallenge/workspace/aichallenge_submit.sh b/aichallenge/workspace/aichallenge_submit.sh deleted file mode 100644 index 4fa784e6..00000000 --- a/aichallenge/workspace/aichallenge_submit.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/bash - -source install/setup.bash - -ros2 launch aichallenge_launch aichallenge_submit.launch.xml \ -use_sim_time:=false \ -launch_vehicle_interface:=true diff --git a/aichallenge/workspace/colcon_build.bash b/aichallenge/workspace/colcon_build.bash index 3e5384c9..a525777f 100755 --- a/aichallenge/workspace/colcon_build.bash +++ b/aichallenge/workspace/colcon_build.bash @@ -1,2 +1,2 @@ #!/usr/bin/bash -colcon build --symlink-install --packages-up-to aichallenge_launch +colcon build --symlink-install --packages-up-to aichallenge_launch --executor sequential diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml index 24d2722d..4b46cba5 100644 --- a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml @@ -224,7 +224,7 @@ - + @@ -233,7 +233,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml new file mode 100644 index 00000000..7e8950db --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml index a66b3510..5a41c0ba 100644 --- a/aichallenge/workspace/src/aichallenge_launch/package.xml +++ b/aichallenge/workspace/src/aichallenge_launch/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake aichallenge_submit_launch + aichallenge_system_launch tier4_vehicle_launch global_parameter_loader goal_pose_setter diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml index 1778c130..ca8b079a 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml @@ -1,15 +1,15 @@ sensor_kit_base_link: gnss_link: - x: 0.0 + x: -0.26 y: 0.0 z: 0.0 roll: 0.0 pitch: 0.0 yaw: 0.0 imu_link: - x: 0.0 + x: 0.85 y: 0.0 z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 + roll: 3.14 + pitch: 0 + yaw: 1.57 From 0d67aeb7c4785c28d36a62444c20fc4bcabbd003 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com> Date: Tue, 27 Aug 2024 17:14:53 +0900 Subject: [PATCH 09/14] fix: add makeflags (#49) Signed-off-by: Masahiro Kubota --- aichallenge/workspace/colcon_build.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/colcon_build.bash b/aichallenge/workspace/colcon_build.bash index a525777f..c9991efd 100755 --- a/aichallenge/workspace/colcon_build.bash +++ b/aichallenge/workspace/colcon_build.bash @@ -1,2 +1,2 @@ #!/usr/bin/bash -colcon build --symlink-install --packages-up-to aichallenge_launch --executor sequential +MAKEFLAGS=-j4 colcon build --symlink-install --packages-up-to aichallenge_launch --executor sequential From dde08f14f866e46336738129b4b4a73c2f6ad4ef Mon Sep 17 00:00:00 2001 From: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com> Date: Wed, 28 Aug 2024 09:43:37 +0900 Subject: [PATCH 10/14] fix: modify sensor configuration (#50) Signed-off-by: Masahiro Kubota --- .../imu_gnss_poser/src/imu_gnss_poser_node.cpp | 8 ++++---- .../config/sensor_kit_calibration.yaml | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp index f48f9707..cdc327fc 100644 --- a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp @@ -26,10 +26,10 @@ class ImuGnssPoser : public rclcpp::Node private: void gnss_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - msg->pose.pose.orientation.x = imu_msg_.orientation.x; - msg->pose.pose.orientation.y = imu_msg_.orientation.y; - msg->pose.pose.orientation.z = imu_msg_.orientation.z; - msg->pose.pose.orientation.w = imu_msg_.orientation.w; + //msg->pose.pose.orientation.x = imu_msg_.orientation.x; + //msg->pose.pose.orientation.y = imu_msg_.orientation.y; + //msg->pose.pose.orientation.z = imu_msg_.orientation.z; + //msg->pose.pose.orientation.w = imu_msg_.orientation.w; msg->pose.covariance[7*0] = 0.1; msg->pose.covariance[7*1] = 0.1; msg->pose.covariance[7*2] = 0.1; diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml index ca8b079a..29ba8499 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml @@ -10,6 +10,6 @@ sensor_kit_base_link: x: 0.85 y: 0.0 z: 0.0 - roll: 3.14 - pitch: 0 - yaw: 1.57 + roll: 0.0 + pitch: 0.0 + yaw: -1.57 From b0bcaf0fc0392c85fad2b1c6bcac5dcf6748dbd0 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com> Date: Wed, 28 Aug 2024 09:44:10 +0900 Subject: [PATCH 11/14] feat: add rviz plugin (#51) * feat: add rviz plugin Signed-off-by: Masahiro Kubota * feat: add rviz plugin Signed-off-by: Masahiro Kubota --------- Signed-off-by: Masahiro Kubota --- .../aichallenge_launch/launch/aichallenge_system.launch.xml | 2 +- aichallenge/workspace/src/aichallenge_launch/package.xml | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml index 7e8950db..d8978772 100644 --- a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge_system.launch.xml @@ -23,6 +23,6 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml index 5a41c0ba..83238406 100644 --- a/aichallenge/workspace/src/aichallenge_launch/package.xml +++ b/aichallenge/workspace/src/aichallenge_launch/package.xml @@ -35,6 +35,11 @@ ad_api_adaptors simple_pure_pursuit + tier4_planning_rviz_plugin + tier4_localization_rviz_plugin + tier4_control_rviz_plugin + tier4_vehicle_rviz_plugin + ament_lint_auto ament_lint_common From c2bc1fa4035c56674fbc697553df7b19a03488a7 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com> Date: Wed, 28 Aug 2024 09:44:37 +0900 Subject: [PATCH 12/14] fix: modify accel map (#52) Signed-off-by: Masahiro Kubota --- .../src/aichallenge_launch/data/accel_map.csv | 2 +- .../aichallenge_submit_launch/data/accel_map.csv | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv index 165e7ee3..979af2be 100644 --- a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv +++ b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv @@ -1,5 +1,5 @@ default,0.0,1.0,2.0,3.0,4.0,5.0 -0.0,0.0,0.0,0.0,0.0,0.0,0.0 +0.0,-0.3,-0.3,-0.3,-0.3,-0.3,-0.3 0.1,0.0,0.0,0.0,0.0,0.0,0.0 0.2,0.0,0.0,0.0,0.0,0.0,0.0 0.3,0.05,0.05,0.05,0.05,0.05,0.05 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv index 32e639ca..979af2be 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv @@ -1,7 +1,8 @@ -default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 -0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 -0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 -0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 -0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 -0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 -0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 +default,0.0,1.0,2.0,3.0,4.0,5.0 +0.0,-0.3,-0.3,-0.3,-0.3,-0.3,-0.3 +0.1,0.0,0.0,0.0,0.0,0.0,0.0 +0.2,0.0,0.0,0.0,0.0,0.0,0.0 +0.3,0.05,0.05,0.05,0.05,0.05,0.05 +0.4,0.1,0.1,0.1,0.1,0.1,0.1 +0.5,0.175,0.175,0.175,0.175,0.175,0.175 +0.6,0.25,0.25,0.25,0.25,0.25,0.25 From 5f403e215cf8617d24047ce8919cc9aef626cfb5 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 2 Oct 2024 19:57:01 +0900 Subject: [PATCH 13/14] revert gnss_callback commentout Signed-off-by: Autumn60 --- .../imu_gnss_poser/src/imu_gnss_poser_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp index cdc327fc..f48f9707 100644 --- a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp @@ -26,10 +26,10 @@ class ImuGnssPoser : public rclcpp::Node private: void gnss_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - //msg->pose.pose.orientation.x = imu_msg_.orientation.x; - //msg->pose.pose.orientation.y = imu_msg_.orientation.y; - //msg->pose.pose.orientation.z = imu_msg_.orientation.z; - //msg->pose.pose.orientation.w = imu_msg_.orientation.w; + msg->pose.pose.orientation.x = imu_msg_.orientation.x; + msg->pose.pose.orientation.y = imu_msg_.orientation.y; + msg->pose.pose.orientation.z = imu_msg_.orientation.z; + msg->pose.pose.orientation.w = imu_msg_.orientation.w; msg->pose.covariance[7*0] = 0.1; msg->pose.covariance[7*1] = 0.1; msg->pose.covariance[7*2] = 0.1; From 07a1187b5f431badd4c148bfc16595b87d7d44eb Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 2 Oct 2024 20:02:17 +0900 Subject: [PATCH 14/14] add decel to .cspell.json (#43) Signed-off-by: Autumn60 --- .cspell.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.cspell.json b/.cspell.json index 2c51a351..f5bfb9fa 100644 --- a/.cspell.json +++ b/.cspell.json @@ -66,6 +66,7 @@ "cyclonedds", "dallara", "dcmake", + "decel", "distro", "downsample", "freespace",