From 932bb4116ffffa87c9a28d97dc1d0e41a431e2a6 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sat, 27 Jul 2024 14:48:55 +0900 Subject: [PATCH 01/48] add sync-upstream ci Signed-off-by: Autumn60 --- .github/workflows/sync-upstream.yaml | 31 ++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 .github/workflows/sync-upstream.yaml diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml new file mode 100644 index 00000000..a2ca196c --- /dev/null +++ b/.github/workflows/sync-upstream.yaml @@ -0,0 +1,31 @@ +name: sync-upstream + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + sync-upstream: + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v2 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run sync-branches + uses: autowarefoundation/autoware-github-actions/sync-branches@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + base-branch: main + sync-pr-branch: sync-upstream + sync-target-repository: https://github.com/AutomotiveAIChallenge/aichallenge-2024.git + sync-target-branch: main + pr-title: "chore: sync upstream" + pr-labels: | + ci + sync-upstream + auto-merge-method: merge \ No newline at end of file From 5f6d7d3c8857b267bd64e73e9efd9b66b4eea0e8 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 28 Jul 2024 13:45:26 +0900 Subject: [PATCH 02/48] add spell-check ci Signed-off-by: Autumn60 --- .cspell.json | 47 +++++++++++++++++++ .../workflows/spell-check-differential.yaml | 18 +++++++ .github/workflows/spell-check.yaml | 20 ++++++++ 3 files changed, 85 insertions(+) create mode 100644 .cspell.json create mode 100644 .github/workflows/spell-check-differential.yaml create mode 100644 .github/workflows/spell-check.yaml diff --git a/.cspell.json b/.cspell.json new file mode 100644 index 00000000..fa91ea5d --- /dev/null +++ b/.cspell.json @@ -0,0 +1,47 @@ +{ + "ignorePaths": [ + "**/*.cspell.json", + "**/*.dae", + "**/*.html", + "**/*.mp3", + "**/*.mp4", + "**/*.pcd", + "**/*.stl", + "**/*.svg", + "**/*.wav", + "**/*.zip", + "**/.git/**", + "**/.gitignore", + "**/.vscode/**", + "**/build/**", + "**/CHANGELOG.rst", + "**/CPPLINT.cfg", + "**/Doxyfile", + "**/install/**", + "**/log/**", + "**/package-lock.json" + ], + "ignoreRegExpList": [ + "\\[.*/.*\\]\\(https://github.com", + "Copyright .*[0-9]{4}.+", + "github.com[/:][\\w._\\-]+(/[\\w._\\-]+)?", + "ppa:.+/[^\\s]+", + "@[a-zA-Z0-9](?:[a-zA-Z0-9]|-(?=[a-zA-Z0-9])){0,38}" + ], + "overrides": [ + { + "filename": "**/*.yaml", + "ignoreRegExpList": ["author: .+$", "git_email: .+$", "git_user: .+$", "uses: .+$"] + }, + { + "filename": "**/package.xml", + "ignoreRegExpList": ["", ""] + }, + { + "filename": "**/{*.cpp,*.hpp}", + "ignoreRegExpList": ["@author .*$", "[\\@]tparam", "\\author .*$", "Author(s)?( )?: .*$"] + } + ], + "words": [ + ] +} diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml new file mode 100644 index 00000000..96dad7c4 --- /dev/null +++ b/.github/workflows/spell-check-differential.yaml @@ -0,0 +1,18 @@ +name: spell-check-differential + +on: + pull_request: + branches: [main] + +jobs: + spell-check-differential: + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Run spell check + uses: streetsidesoftware/cspell-action@v6 + with: + config: .cspell.json + incremental_files_only: true diff --git a/.github/workflows/spell-check.yaml b/.github/workflows/spell-check.yaml new file mode 100644 index 00000000..38046097 --- /dev/null +++ b/.github/workflows/spell-check.yaml @@ -0,0 +1,20 @@ +name: spell-check + +on: + push: + branches: + - main + workflow_dispatch: + +jobs: + spell-check: + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Run spell check + uses: streetsidesoftware/cspell-action@v6 + with: + config: .cspell.json + incremental_files_only: false From ad0f7b8a0fa0f3dd37b2c7a65e0ac056089a25cb Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 28 Jul 2024 13:56:02 +0900 Subject: [PATCH 03/48] update pr-labels in sync-upstream.yaml Signed-off-by: Autumn60 --- .github/workflows/sync-upstream.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml index a2ca196c..bc7ea6ca 100644 --- a/.github/workflows/sync-upstream.yaml +++ b/.github/workflows/sync-upstream.yaml @@ -26,6 +26,6 @@ jobs: sync-target-branch: main pr-title: "chore: sync upstream" pr-labels: | - ci + bot sync-upstream auto-merge-method: merge \ No newline at end of file From d8f9149e662822efd84284c27861fc4b25532684 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Sun, 28 Jul 2024 19:28:42 +0900 Subject: [PATCH 04/48] ci(spell check): add words to dict (#5) * add words to .cspell.json Signed-off-by: Autumn60 * add .rviz to ignore path list Signed-off-by: Autumn60 * add .sh to ignore path list Signed-off-by: Autumn60 * add .bash to ignore path list Signed-off-by: Autumn60 * add words to dict Signed-off-by: Autumn60 * chore: Add "TODO" to ignoreRegExpList in .cspell.json Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .cspell.json | 61 +++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/.cspell.json b/.cspell.json index fa91ea5d..1c58d051 100644 --- a/.cspell.json +++ b/.cspell.json @@ -1,11 +1,14 @@ { "ignorePaths": [ + "**/*.bash", "**/*.cspell.json", "**/*.dae", "**/*.html", "**/*.mp3", "**/*.mp4", "**/*.pcd", + "**/*.rviz", + "**/*.sh", "**/*.stl", "**/*.svg", "**/*.wav", @@ -39,9 +42,65 @@ }, { "filename": "**/{*.cpp,*.hpp}", - "ignoreRegExpList": ["@author .*$", "[\\@]tparam", "\\author .*$", "Author(s)?( )?: .*$"] + "ignoreRegExpList": ["@author .*$", "[\\@]tparam", "\\author .*$", "Author(s)?( )?: .*$", "TODO\\((.*?)\\)"] } ], "words": [ + "ackermann", + "adapi", + "aichallenge", + "autocompute", + "automotiveaichallenge", + "autoware", + "awsim", + "buildtool", + "colcon", + "costmap", + "cuda", + "cyclonedds", + "dallara", + "dcmake", + "distro", + "downsample", + "freespace", + "gnss", + "gnucxx", + "gtest", + "initialpose", + "lanechange", + "lanefollowing", + "lanelet", + "libgazebo", + "libgl", + "linalg", + "lowpass", + "mapfile", + "mathcal", + "mgrs", + "odometry", + "osrf", + "pitstop", + "pointcloud", + "rclcpp", + "rclpy", + "rcutils", + "rgba", + "rois", + "rosdep", + "rosdistro", + "rviz", + "schematypens", + "sideshift", + "srvs", + "stddev", + "traj", + "urdf", + "velodyne", + "wextra", + "wmctrl", + "wpedantic", + "xacro", + "xyzrpy", + "zcvf" ] } From 1526f0884a93d84f6c9e16edf680ca741f6aa4c8 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Sun, 11 Aug 2024 18:23:36 +0900 Subject: [PATCH 05/48] chore: add .gitignore and .clang-format (#9) * update .gitignore Signed-off-by: Autumn60 * add .clang-format Signed-off-by: Autumn60 * add .clang-format to ignore list in .cspell.json Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .cspell.json | 1 + aichallenge/workspace/.clang-format | 47 +++++++++++++++++++++++++++++ aichallenge/workspace/.gitignore | 3 ++ 3 files changed, 51 insertions(+) create mode 100644 aichallenge/workspace/.clang-format diff --git a/.cspell.json b/.cspell.json index 1c58d051..550d2344 100644 --- a/.cspell.json +++ b/.cspell.json @@ -18,6 +18,7 @@ "**/.vscode/**", "**/build/**", "**/CHANGELOG.rst", + "**/.clang-format", "**/CPPLINT.cfg", "**/Doxyfile", "**/install/**", diff --git a/aichallenge/workspace/.clang-format b/aichallenge/workspace/.clang-format new file mode 100644 index 00000000..b41fae91 --- /dev/null +++ b/aichallenge/workspace/.clang-format @@ -0,0 +1,47 @@ +# Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +AllowShortFunctionsOnASingleLine: InlineOnly +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: true +IncludeCategories: + # C++ system headers + - Regex: <[a-z_]*> + Priority: 6 + CaseSensitive: true + # C system headers + - Regex: <.*\.h> + Priority: 5 + CaseSensitive: true + # Boost headers + - Regex: boost/.* + Priority: 4 + CaseSensitive: true + # Message headers + - Regex: .*_msgs/.* + Priority: 3 + CaseSensitive: true + - Regex: .*_srvs/.* + Priority: 3 + CaseSensitive: true + # Other Package headers + - Regex: <.*> + Priority: 2 + CaseSensitive: true + # Local package headers + - Regex: '".*"' + Priority: 1 + CaseSensitive: true diff --git a/aichallenge/workspace/.gitignore b/aichallenge/workspace/.gitignore index 4c288e3e..ab2524be 100644 --- a/aichallenge/workspace/.gitignore +++ b/aichallenge/workspace/.gitignore @@ -1,3 +1,6 @@ /build /install /log + +.vscode +*.code-workspace \ No newline at end of file From 3a3d14f8e5295a32a053d8e1f664d8063ffdd690 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Sun, 11 Aug 2024 18:23:47 +0900 Subject: [PATCH 06/48] add "booars" to word list in .cspell.json (#11) Signed-off-by: Autumn60 --- .cspell.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.cspell.json b/.cspell.json index 550d2344..8e48ac4a 100644 --- a/.cspell.json +++ b/.cspell.json @@ -54,6 +54,7 @@ "automotiveaichallenge", "autoware", "awsim", + "booars", "buildtool", "colcon", "costmap", From c207fced5359c7bbc32a8dfb016b0283ffaab615 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 21 Aug 2024 18:11:14 +0900 Subject: [PATCH 07/48] feat(common): create booars_utils pkg and create function_timer.hpp (#10) * create pkg directory Signed-off-by: Autumn60 * create function_timer.hpp Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .../booars_utils/CMakeLists.txt | 19 +++++++ .../booars_utils/ros/function_timer.hpp | 52 +++++++++++++++++++ .../booars_utils/package.xml | 20 +++++++ 3 files changed, 91 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt new file mode 100644 index 00000000..693dcc12 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_utils) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +include_directories(include) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp new file mode 100644 index 00000000..56935e23 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 Booars +// +// 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 BOOARS_UTILS__ROS__FUNCTION_TIMER_HPP_ +#define BOOARS_UTILS__ROS__FUNCTION_TIMER_HPP_ + +#include +#include +#include + +namespace booars_utils::ros { + +class FunctionTimer { + private: + rclcpp::TimerBase::SharedPtr timer_; + + public: + using SharedPtr = std::shared_ptr; + + static SharedPtr create_function_timer(rclcpp::Node* node, + const double update_rate_hz, + std::function callback) { + return std::make_shared(node, update_rate_hz, callback); + } + + explicit FunctionTimer(rclcpp::Node* node, const double update_rate_hz, + std::function callback) { + const double dt = 1.0 / update_rate_hz; + + auto period = std::chrono::duration_cast( + std::chrono::duration(dt)); + timer_ = std::make_shared>( + node->get_clock(), period, std::move(callback), + node->get_node_base_interface()->get_context()); + node->get_node_timers_interface()->add_timer(timer_, nullptr); + } +}; + +} // namespace booars_utils::ros + +#endif // BOOARS_UTILS__ROS__FUNCTION_TIMER_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml new file mode 100644 index 00000000..5bb22bbe --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml @@ -0,0 +1,20 @@ + + + + booars_utils + 0.1.0 + The booars_utils package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + rclcpp + + ament_lint_auto + + + ament_cmake + + From b074fc0a250e0d70faf883ad013a5f372bbcbe83 Mon Sep 17 00:00:00 2001 From: sitahara <33857284+sitahara@users.noreply.github.com> Date: Wed, 21 Aug 2024 22:18:44 +0900 Subject: [PATCH 08/48] feat(control): improved pure pursuit control (#12) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: pure_pursuitの見ている点をrviz2で可視化 * pure_pursuitのパラメタ調整 * feat: added rviz2 display setting * Revert "pure_pursuitのパラメタ調整" This reverts commit 6e45cedfae7c0098eb00af5c35045299019ab2a9. --------- Co-authored-by: Shotaro Itahara --- .../simple_pure_pursuit.hpp | 18 ++++--- .../src/simple_pure_pursuit.cpp | 51 ++++++++++++++++++- .../config/autoware.rviz | 13 +++++ 3 files changed, 75 insertions(+), 7 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp index e9433945..8d82262d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp @@ -9,6 +9,8 @@ #include #include #include +#include +#include namespace simple_pure_pursuit { @@ -18,6 +20,7 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; +using visualization_msgs::msg::Marker; class SimplePurePursuit : public rclcpp::Node { public: @@ -29,6 +32,7 @@ class SimplePurePursuit : public rclcpp::Node { // publishers rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr mkr_cmd_; // timer rclcpp::TimerBase::SharedPtr timer_; @@ -40,17 +44,19 @@ class SimplePurePursuit : public rclcpp::Node { // pure pursuit parameters - const double wheel_base_; - const double lookahead_gain_; - const double lookahead_min_distance_; - const double speed_proportional_gain_; - const bool use_external_target_vel_; - const double external_target_vel_; + double wheel_base_; + double lookahead_gain_; + double lookahead_min_distance_; + double speed_proportional_gain_; + bool use_external_target_vel_; + double external_target_vel_; + OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; private: void onTimer(); bool subscribeMessageAvailable(); + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters); }; } // namespace simple_pure_pursuit diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index aa3ac2b1..736185e1 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -25,6 +25,7 @@ SimplePurePursuit::SimplePurePursuit() external_target_vel_(declare_parameter("external_target_vel", 0.0)) { pub_cmd_ = create_publisher("output/control_cmd", 1); + mkr_cmd_ = create_publisher("debug/pursuit_lookahead", 1); sub_kinematics_ = create_subscription( "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); @@ -34,6 +35,10 @@ SimplePurePursuit::SimplePurePursuit() using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 30ms, std::bind(&SimplePurePursuit::onTimer, this)); + + // dynamic reconfigure + auto parameter_change_cb = std::bind(&SimplePurePursuit::parameter_callback, this, std::placeholders::_1); + reset_param_handler_ = SimplePurePursuit::add_on_set_parameters_callback(parameter_change_cb); } AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp) @@ -100,7 +105,31 @@ void SimplePurePursuit::onTimer() } double lookahead_point_x = lookahead_point_itr->pose.position.x; double lookahead_point_y = lookahead_point_itr->pose.position.y; - + { + // publish lookahead point marker + auto marker_msg = Marker(); + marker_msg.header.frame_id = "map"; + marker_msg.header.stamp = now(); + marker_msg.ns = "basic_shapes"; + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::SPHERE; + marker_msg.action = visualization_msgs::msg::Marker::ADD; + marker_msg.pose.position.x = lookahead_point_x; + marker_msg.pose.position.y = lookahead_point_y; + marker_msg.pose.position.z = 80; + marker_msg.pose.orientation.x = 0.0; + marker_msg.pose.orientation.y = 0.0; + marker_msg.pose.orientation.z = 0.0; + marker_msg.pose.orientation.w = 1.0; + marker_msg.scale.x = 3.0; + marker_msg.scale.y = 3.0; + marker_msg.scale.z = 3.0; + marker_msg.color.r = 1.0f; + marker_msg.color.g = 0.0f; + marker_msg.color.b = 0.0f; + marker_msg.color.a = 1.0; + mkr_cmd_->publish(marker_msg); + } // calc steering angle for lateral control double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - tf2::getYaw(odometry_->pose.pose.orientation); @@ -122,8 +151,28 @@ bool SimplePurePursuit::subscribeMessageAvailable() } return true; } +rcl_interfaces::msg::SetParametersResult SimplePurePursuit::parameter_callback(const std::vector ¶meters){ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "lookahead_gain") { + lookahead_gain_ = parameter.as_double(); + RCLCPP_INFO(SimplePurePursuit::get_logger(), "lookahead_gain changed to %f", lookahead_gain_); + } else if (parameter.get_name() == "lookahead_min_distance") { + lookahead_min_distance_ = parameter.as_double(); + RCLCPP_INFO(SimplePurePursuit::get_logger(), "lookahead_min_distance changed to %f", lookahead_min_distance_); + } else if (parameter.get_name() == "external_target_vel") { + external_target_vel_ = parameter.as_double(); + RCLCPP_INFO(SimplePurePursuit::get_logger(), "external_target_vel changed to %f", external_target_vel_); + } + } + return result; +} } // namespace simple_pure_pursuit + + int main(int argc, char const * argv[]) { rclcpp::init(argc, argv); diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index c76a8956..1539328f 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -1769,6 +1769,19 @@ Visualization Manager: Reliability Policy: Reliable Value: /aichallenge/pitstop/area_marker Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /debug/pursuit_lookahead + Value: true Enabled: true Global Options: Background Color: 10; 10; 10 From 81ceafe2069bcc72588843230826d6bce3008a6d Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 21 Aug 2024 23:56:21 +0900 Subject: [PATCH 09/48] create CODEOWNERS (#18) Signed-off-by: Autumn60 --- .github/CODEOWNERS | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .github/CODEOWNERS diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 00000000..6b1ec347 --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,11 @@ +aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60 +aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/** @hrjp +aichallenge/workspace/src/aichallenge_submit/gyro_odometer/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/imu_corrector/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/racing_kart_description/** @Autumn60 +aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara +aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers From 1859c6a2d75a354590ac6ac5eb47d09460966532 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Fri, 23 Aug 2024 22:51:45 +0900 Subject: [PATCH 10/48] add "lanelets" to words list in .cspell.json (#17) Signed-off-by: Autumn60 --- .cspell.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.cspell.json b/.cspell.json index 8e48ac4a..3c1715b8 100644 --- a/.cspell.json +++ b/.cspell.json @@ -72,6 +72,7 @@ "lanechange", "lanefollowing", "lanelet", + "lanelets", "libgazebo", "libgl", "linalg", From 0ea6e14535e75fd8e4893ba5380cb5d0b0502bd7 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Sat, 24 Aug 2024 14:05:29 +0900 Subject: [PATCH 11/48] feat(launch): create booars launch (#19) * create booars_launch Signed-off-by: Autumn60 * replace reference.launch.xml to booars.launch.xml Signed-off-by: Autumn60 * update CODEOWNERS Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .github/CODEOWNERS | 1 + .../launch/aichallenge_submit.launch.xml | 6 +- .../booars_launch/CMakeLists.txt | 6 + .../config/sensing/imu_corrector.param.yaml | 8 + .../booars_launch/launch/booars.launch.xml | 54 + .../launch/components/common.launch.xml | 26 + .../launch/components/control.launch.xml | 17 + .../launch/components/localization.launch.xml | 44 + .../launch/components/map.launch.xml | 41 + .../launch/components/perception.launch.xml | 9 + .../launch/components/planning.launch.xml | 104 + .../launch/components/sensing.launch.xml | 25 + .../launch/components/vehicle.launch.xml | 6 + .../booars_launch/map/lanelet2_map.osm | 6025 +++++++++++++++++ .../booars_launch/package.xml | 18 + 15 files changed, 6385 insertions(+), 5 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/imu_corrector.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/common.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/package.xml diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6b1ec347..d5d743c9 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,5 @@ aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/booars_launch/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/** @hrjp aichallenge/workspace/src/aichallenge_submit/gyro_odometer/** @booars/aic2024-developers diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml index bd904eab..510f861c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml @@ -1,11 +1,7 @@ - - - - - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt new file mode 100644 index 00000000..6440f8e3 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() +ament_auto_package(INSTALL_TO_SHARE launch map config) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/imu_corrector.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/imu_corrector.param.yaml new file mode 100644 index 00000000..24405b3c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/imu_corrector.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + angular_velocity_offset_x: 0.0 # [rad/s] + angular_velocity_offset_y: 0.0 # [rad/s] + angular_velocity_offset_z: 0.0 # [rad/s] + angular_velocity_stddev_xx: 0.03 # [rad/s] + angular_velocity_stddev_yy: 0.03 # [rad/s] + angular_velocity_stddev_zz: 0.03 # [rad/s] \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml new file mode 100644 index 00000000..642e91c8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/common.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/common.launch.xml new file mode 100644 index 00000000..972982fd --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/common.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml new file mode 100644 index 00000000..f23f0812 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml new file mode 100644 index 00000000..ab77ad89 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml new file mode 100644 index 00000000..e5d3e09d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml new file mode 100644 index 00000000..c2338819 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml new file mode 100644 index 00000000..2c8c989f --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml new file mode 100644 index 00000000..403f1b21 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml new file mode 100644 index 00000000..97d01413 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm new file mode 100644 index 00000000..0c9222a4 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm @@ -0,0 +1,6025 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/package.xml new file mode 100644 index 00000000..638899bc --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/package.xml @@ -0,0 +1,18 @@ + + + booars_launch + 0.1.0 + The booars_launch package + + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + From 32ebd37ee07651fc54334a8ad6414b8f1a6c73f2 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Tue, 27 Aug 2024 21:28:30 +0900 Subject: [PATCH 12/48] feat(perception): dummy perception publisher (#20) * create booars_dummy_perception_publisher pkg Signed-off-by: Autumn60 * replace empty_objects_publisher to dummy_objects_publisher Signed-off-by: Autumn60 * update CODEOWNERS Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .github/CODEOWNERS | 1 + .../CMakeLists.txt | 33 ++++++++++ .../dummy_objects_publisher.hpp | 45 +++++++++++++ .../package.xml | 21 ++++++ .../src/dummy_objects_publisher.cpp | 66 +++++++++++++++++++ .../launch/components/perception.launch.xml | 3 +- 6 files changed, 168 insertions(+), 1 deletion(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/include/booars_dummy_perception_publisher/dummy_objects_publisher.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/src/dummy_objects_publisher.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d5d743c9..b0417909 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,5 @@ aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/booars_launch/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/** @hrjp diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/CMakeLists.txt new file mode 100644 index 00000000..d43b8ac3 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_dummy_perception_publisher) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +include_directories(include) + +ament_auto_add_library(booars_dummy_perception_publisher SHARED + src/dummy_objects_publisher.cpp +) + +rclcpp_components_register_node(booars_dummy_perception_publisher + PLUGIN "booars_dummy_perception_publisher::DummyObjectsPublisher" + EXECUTABLE dummy_objects_publisher +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/include/booars_dummy_perception_publisher/dummy_objects_publisher.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/include/booars_dummy_perception_publisher/dummy_objects_publisher.hpp new file mode 100644 index 00000000..6d588a1c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/include/booars_dummy_perception_publisher/dummy_objects_publisher.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Booars +// +// 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 BOOARS_DUMMY_PERCEPTION_PUBLISHER__DUMMY_OBJECTS_PUBLISHER_HPP_ +#define BOOARS_DUMMY_PERCEPTION_PUBLISHER__DUMMY_OBJECTS_PUBLISHER_HPP_ + +#include + +#include +#include + +#include + +namespace booars_dummy_perception_publisher +{ +using Float64MultiArray = std_msgs::msg::Float64MultiArray; +using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + +class DummyObjectsPublisher : public rclcpp::Node +{ +public: + explicit DummyObjectsPublisher(const rclcpp::NodeOptions & options); + +private: + void objects_callback(const Float64MultiArray::SharedPtr msg); + + rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Publisher::SharedPtr objects_pub_; + + std::string map_frame_id_; +}; +} // namespace booars_dummy_perception_publisher + +#endif // BOOARS_DUMMY_PERCEPTION_PUBLISHER__DUMMY_OBJECTS_PUBLISHER_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml new file mode 100644 index 00000000..9ab26623 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/package.xml @@ -0,0 +1,21 @@ + + + + booars_dummy_perception_publisher + 0.0.0 + The booars_dummy_perception_publisher package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + autoware_auto_perception_msgs + rclcpp_components + std_msgs + ament_lint_auto + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/src/dummy_objects_publisher.cpp b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/src/dummy_objects_publisher.cpp new file mode 100644 index 00000000..0c639e3c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/src/dummy_objects_publisher.cpp @@ -0,0 +1,66 @@ +// Copyright 2024 Booars +// +// 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 "booars_dummy_perception_publisher/dummy_objects_publisher.hpp" + +namespace booars_dummy_perception_publisher +{ +DummyObjectsPublisher::DummyObjectsPublisher(const rclcpp::NodeOptions & options) +: Node("dummy_objects_publisher", options) +{ + map_frame_id_ = declare_parameter("map_frame_id", "map"); + + objects_sub_ = create_subscription( + "~/input/objects", 10, + std::bind(&DummyObjectsPublisher::objects_callback, this, std::placeholders::_1)); + objects_pub_ = create_publisher("~/output/objects", 10); +} + +void DummyObjectsPublisher::objects_callback(const Float64MultiArray::SharedPtr msg) +{ + if (msg->data.size() % 4 != 0) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, "Invalid message size"); + return; + } + int object_count = msg->data.size() / 4; + + PredictedObjects objects_msg; + + for (int i = 0; i < object_count; i++) { + autoware_auto_perception_msgs::msg::PredictedObject object; + + object.object_id.uuid[0] = i; + + object.kinematics.initial_pose_with_covariance.pose.position.x = msg->data[i * 4 + 0]; + object.kinematics.initial_pose_with_covariance.pose.position.y = msg->data[i * 4 + 1]; + object.kinematics.initial_pose_with_covariance.pose.position.z = msg->data[i * 4 + 2]; + object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1.0; + + object.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + const double size = msg->data[i * 4 + 3] * 2.0; + object.shape.dimensions.x = size; + object.shape.dimensions.y = size; + object.shape.dimensions.z = 1.0; + + objects_msg.objects.push_back(object); + } + + objects_msg.header.frame_id = map_frame_id_; + objects_msg.header.stamp = now(); + objects_pub_->publish(objects_msg); +} +} // namespace booars_dummy_perception_publisher + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(booars_dummy_perception_publisher::DummyObjectsPublisher) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml index c2338819..a16e4d85 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml @@ -2,7 +2,8 @@ - + + From 839feb6610db6395ed5c111a3e7b12960c18308e Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 29 Aug 2024 17:05:05 +0900 Subject: [PATCH 13/48] feat(planning): create multi-layered costmap system (#21) * create occupancy_grid utils Signed-off-by: Autumn60 * add update_origin func to occupancy_grid_utils.hpp Signed-off-by: Autumn60 (cherry picked from commit 96000cbb96377321fee819f19033eea061c37c3c) * create multi_layered_costmap and costmap_base Signed-off-by: Autumn60 * create lanelet_costmap Signed-off-by: Autumn60 * add dummy subscriber to lanelet_costmap Signed-off-by: Autumn60 * fix license notation Signed-off-by: Autumn60 * fix depend Signed-off-by: Autumn60 * replace throw to return Signed-off-by: Autumn60 * create predicted_object_costmap Signed-off-by: Autumn60 * add create_costmap func to lanelet_costmap Signed-off-by: Autumn60 * add create_costmap func to predicted_object_costmap Signed-off-by: Autumn60 * add SharedPtr to MultiLayeredCostmap Signed-off-by: Autumn60 * create booars_costmap_utils Signed-off-by: Autumn60 * create multi_layered_costmap_visualizer Signed-off-by: Autumn60 * fix virtual function Signed-off-by: Autumn60 * add lanelet_costmap initialization Signed-off-by: Autumn60 * add get_parameter func for costmap layers Signed-off-by: Autumn60 * fix param.yaml Signed-off-by: Autumn60 * delete unnecessary space Signed-off-by: Autumn60 * rename pkg Signed-off-by: Autumn60 * update costmap_generator Signed-off-by: Autumn60 * add get_index_to_table func to occupancy_grid_utils Signed-off-by: Autumn60 * fix MOROMORO Signed-off-by: Autumn60 * update .param.yaml Signed-off-by: Autumn60 * update CODEOWNERS Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .github/CODEOWNERS | 1 + .../nav/occupancy_grid_parameters.hpp | 64 +++++++++ .../booars_utils/nav/occupancy_grid_utils.hpp | 85 ++++++++++++ .../booars_utils/package.xml | 2 + .../booars_costmap_generator/CMakeLists.txt | 34 +++++ .../config/costmap_generator.param.yaml | 18 +++ .../costmap_generator.hpp | 68 ++++++++++ .../launch/costmap_generator.launch.xml | 7 + .../booars_costmap_generator/package.xml | 25 ++++ .../src/costmap_generator.cpp | 122 ++++++++++++++++++ .../booars_costmap_utils/CMakeLists.txt | 17 +++ .../booars_costmap_utils.hpp | 55 ++++++++ .../costmap/booars_costmap_utils/package.xml | 20 +++ .../costmap/lanelet_costmap/CMakeLists.txt | 16 +++ .../lanelet_costmap/lanelet_costmap.hpp | 65 ++++++++++ .../costmap/lanelet_costmap/package.xml | 24 ++++ .../lanelet_costmap/src/lanelet_costmap.cpp | 84 ++++++++++++ .../multi_layered_costmap/CMakeLists.txt | 16 +++ .../multi_layered_costmap/costmap_base.hpp | 35 +++++ .../multi_layered_costmap.hpp | 37 ++++++ .../costmap/multi_layered_costmap/package.xml | 18 +++ .../src/multi_layered_costmap.cpp | 40 ++++++ .../predicted_object_costmap/CMakeLists.txt | 16 +++ .../predicted_object_costmap.hpp | 65 ++++++++++ .../predicted_object_costmap/package.xml | 21 +++ .../src/predicted_object_costmap.cpp | 95 ++++++++++++++ 26 files changed, 1050 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/include/lanelet_costmap/lanelet_costmap.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/src/lanelet_costmap.cpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/costmap_base.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/multi_layered_costmap.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/src/multi_layered_costmap.cpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/include/predicted_object_costmap/predicted_object_costmap.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/src/predicted_object_costmap.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b0417909..379992ee 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,6 +2,7 @@ aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booar aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/booars_launch/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60 +aichallenge/workspace/src/aichallenge_submit/costmap/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/** @hrjp aichallenge/workspace/src/aichallenge_submit/gyro_odometer/** @booars/aic2024-developers aichallenge/workspace/src/aichallenge_submit/imu_corrector/** @booars/aic2024-developers diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp new file mode 100644 index 00000000..b6ccaddf --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp @@ -0,0 +1,64 @@ +// Copyright 2024 Booars +// +// 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 BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_ +#define BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_ + +#include + +namespace booars_utils::nav +{ +class OccupancyGridParameters +{ +public: + using SharedPtr = std::shared_ptr; + + static OccupancyGridParameters::SharedPtr create_parameters( + const double width, const double resolution) + { + return std::make_shared(width, resolution); + } + + explicit OccupancyGridParameters(const double width, const double resolution) + { + width_ = width; + width_2_ = width / 2.0; + resolution_ = resolution; + resolution_inv_ = 1.0 / resolution; + grid_width_2_ = static_cast(width * resolution_inv_) / 2; + grid_width_ = grid_width_2_ * 2; + grid_num_ = grid_width_ * grid_width_; + } + + double width() const { return width_; } + double width_2() const { return width_2_; } + double resolution() const { return resolution_; } + double resolution_inv() const { return resolution_inv_; } + int grid_width_2() const { return grid_width_2_; } + int grid_width() const { return grid_width_; } + int grid_num() const { return grid_num_; } + +private: + double width_; + double width_2_; + double resolution_; + double resolution_inv_; + int grid_width_2_; + int grid_width_; + int grid_num_; +}; + +} // namespace booars_utils::nav + +#endif // BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp new file mode 100644 index 00000000..61af7876 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 Booars +// +// 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 BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ +#define BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ + +#include "booars_utils/nav/occupancy_grid_parameters.hpp" + +#include + +#include + +namespace booars_utils::nav::occupancy_grid_utils +{ +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; + +OccupancyGridParameters::SharedPtr create_occupancy_grid_parameters( + const OccupancyGrid::SharedPtr occupancy_grid) +{ + const double width = occupancy_grid->info.width * occupancy_grid->info.resolution; + return OccupancyGridParameters::create_parameters(width, occupancy_grid->info.resolution); +} + +OccupancyGrid::SharedPtr create_occupancy_grid( + const OccupancyGridParameters::SharedPtr parameters, const int8_t value = 0) +{ + OccupancyGrid::SharedPtr occupancy_grid = std::make_shared(); + occupancy_grid->info.width = parameters->grid_width(); + occupancy_grid->info.height = parameters->grid_width(); + occupancy_grid->info.resolution = parameters->resolution(); + occupancy_grid->info.origin.position.x = -parameters->width_2(); + occupancy_grid->info.origin.position.y = -parameters->width_2(); + occupancy_grid->data.resize(parameters->grid_num(), value); + return occupancy_grid; +} + +void update_origin( + OccupancyGrid::SharedPtr occupancy_grid, const OccupancyGridParameters::SharedPtr parameters, + const geometry_msgs::msg::Vector3 & translation) +{ + occupancy_grid->info.origin.position.x = -parameters->width_2() + translation.x; + occupancy_grid->info.origin.position.y = -parameters->width_2() + translation.y; + occupancy_grid->info.origin.position.z = translation.z; + occupancy_grid->info.origin.orientation.x = 0.0; + occupancy_grid->info.origin.orientation.y = 0.0; + occupancy_grid->info.origin.orientation.z = 0.0; + occupancy_grid->info.origin.orientation.w = 1.0; +} + +tier4_autoware_utils::Point2d index_to_point( + const OccupancyGridParameters::SharedPtr parameters, const int index) +{ + const int index_x = index % parameters->grid_width(); + const int index_y = index / parameters->grid_width(); + + return { + index_x * parameters->resolution() - parameters->width_2(), + index_y * parameters->resolution() - parameters->width_2()}; +} + +std::vector get_index_to_point_table( + const OccupancyGridParameters::SharedPtr parameters) +{ + std::vector table; + table.reserve(parameters->grid_num()); + for (int i = 0; i < parameters->grid_num(); ++i) { + table.push_back(index_to_point(parameters, i)); + } + return table; +} + +} // namespace booars_utils::nav::occupancy_grid_utils + +#endif // BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml index 5bb22bbe..0db5a693 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml @@ -10,7 +10,9 @@ ament_cmake_auto + nav_msgs rclcpp + tier4_autoware_utils ament_lint_auto diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt new file mode 100644 index 00000000..b1158743 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_costmap_generator) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(costmap_generator SHARED + src/costmap_generator.cpp +) + +rclcpp_components_register_node(costmap_generator + PLUGIN "booars_costmap_generator::CostmapGenerator" + EXECUTABLE costmap_generator_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml new file mode 100644 index 00000000..8173e2a4 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + update_rate: 10.0 + map_frame_id: "map" + costmap_center_frame_id: "base_link" + costmap_width: 20.0 + costmap_resolution: 0.1 + multi_layered_costmap: + layers: + - "lanelet_layer" + - "predicted_object_layer" + lanelet_layer: + type: "lanelet" + map_topic: "/map/vector_map" + predicted_object_layer: + type: "predicted_object" + predicted_objects_topic: "/perception/object_recognition/objects" + distance_threshold: 3.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp new file mode 100644 index 00000000..ce942902 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp @@ -0,0 +1,68 @@ +// Copyright 2024 Booars +// +// 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 BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#define BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace booars_costmap_generator +{ + +using FunctionTimer = booars_utils::ros::FunctionTimer; +using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap; +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; +using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters; +using PointStamped = geometry_msgs::msg::PointStamped; +using Point2d = tier4_autoware_utils::Point2d; +using Vector3 = geometry_msgs::msg::Vector3; + +class CostmapGenerator : public rclcpp::Node +{ +public: + explicit CostmapGenerator(const rclcpp::NodeOptions & options); + +private: + void update(); + bool try_get_transform(geometry_msgs::msg::TransformStamped & transform); + + FunctionTimer::SharedPtr update_timer_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Publisher::SharedPtr costmap_pub_; + MultiLayeredCostmap::SharedPtr multi_layered_costmap_; + + OccupancyGrid::SharedPtr costmap_; + OccupancyGridParameters::SharedPtr costmap_parameters_; + + std::string map_frame_; + std::string target_frame_; + + std::vector index_to_point_table_; +}; +}; // namespace booars_costmap_generator + +#endif // BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml new file mode 100644 index 00000000..9501d94e --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml new file mode 100644 index 00000000..03a0188d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml @@ -0,0 +1,25 @@ + + + + booars_costmap_generator + 0.1.0 + The booars_costmap_generator package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + booars_costmap_utils + booars_utils + rclcpp + rclcpp_components + tf2_ros + tier4_autoware_utils + + ament_lint_auto + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp new file mode 100644 index 00000000..aa86eee8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 Booars +// +// 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 "booars_costmap_generator/costmap_generator.hpp" + +#include + +namespace booars_costmap_generator +{ +CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & options) +: Node("costmap_generator", options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) +{ + // Declare parameters + { + map_frame_ = this->declare_parameter("map_frame_id", "map"); + target_frame_ = this->declare_parameter("costmap_center_frame_id", "base_link"); + } + + // Create publisher + { + costmap_pub_ = this->create_publisher("~/output/costmap", 1); + } + + // Crate update() timer + { + double update_rate = this->declare_parameter("update_rate", 10.0); + update_timer_ = FunctionTimer::create_function_timer( + this, update_rate, std::bind(&CostmapGenerator::update, this)); + } + + // Declare costmap parameters + { + double costmap_width = this->declare_parameter("costmap_width", 10.0); + double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); + costmap_parameters_ = + OccupancyGridParameters::create_parameters(costmap_width, costmap_resolution); + index_to_point_table_ = + booars_utils::nav::occupancy_grid_utils::get_index_to_point_table(costmap_parameters_); + } + + // Create costmap + { + costmap_ = booars_utils::nav::occupancy_grid_utils::create_occupancy_grid(costmap_parameters_); + costmap_->header.frame_id = map_frame_; + } + + // Crate multi layered costmap + { + multi_layered_costmap_ = + booars_costmap_utils::create_multi_layered_costmap(*this, "multi_layered_costmap"); + } +} + +void CostmapGenerator::update() +{ + if (!multi_layered_costmap_->is_ready()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "MultiLayeredCostmap is not ready"); + return; + } + + rclcpp::Time costmap_time; + Vector3 costmap_center_position; + { + geometry_msgs::msg::TransformStamped transform; + if (!try_get_transform(transform)) return; + costmap_time = transform.header.stamp; + costmap_center_position = transform.transform.translation; + } + + // Fill the costmap data + { + for (int i = 0; i < costmap_parameters_->grid_num(); i++) { + Point2d local_point = index_to_point_table_[i]; + PointStamped global_point; + global_point.header.frame_id = map_frame_; + global_point.header.stamp = costmap_time; + global_point.point.x = local_point[0] + costmap_center_position.x; + global_point.point.y = local_point[1] + costmap_center_position.y; + global_point.point.z = 0.0; + costmap_->data[i] = multi_layered_costmap_->is_occupied(global_point) ? 100 : 0; + } + } + + // Update the costmap origin + { + booars_utils::nav::occupancy_grid_utils::update_origin( + costmap_, costmap_parameters_, costmap_center_position); + } + + // Publish the costmap + { + costmap_->header.stamp = costmap_time; + costmap_pub_->publish(*costmap_); + } +} + +bool CostmapGenerator::try_get_transform(geometry_msgs::msg::TransformStamped & transform) +{ + try { + transform = tf_buffer_.lookupTransform(map_frame_, target_frame_, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Failed to get transform: %s", ex.what()); + return false; + } + return true; +} +} // namespace booars_costmap_generator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(booars_costmap_generator::CostmapGenerator) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/CMakeLists.txt new file mode 100644 index 00000000..44ecaf10 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_costmap_utils) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp new file mode 100644 index 00000000..dc4ef26d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp @@ -0,0 +1,55 @@ +// Copyright 2024 Booars +// +// 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 BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ +#define BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ + +#include +#include +#include + +namespace booars_costmap_utils +{ +using LaneletCostmap = lanelet_costmap::LaneletCostmap; +using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap; +using PredictedObjectCostmap = predicted_object_costmap::PredictedObjectCostmap; + +MultiLayeredCostmap::SharedPtr create_multi_layered_costmap( + rclcpp::Node & node, const std::string & costmap_namespace) +{ + auto costmap = std::make_shared(); + + auto layers = node.declare_parameter(costmap_namespace + ".layers", std::vector()); + + for (const auto & layer : layers) { + std::string layer_namespace = costmap_namespace + "." + layer; + auto type = node.declare_parameter(layer_namespace + ".type", std::string()); + + if (type == "lanelet") { + auto lanelet_costmap = LaneletCostmap::create_costmap(node, layer_namespace); + costmap->add_costmap_layer(lanelet_costmap); + } else if (type == "predicted_object") { + auto predicted_object_costmap = PredictedObjectCostmap::create_costmap(node, layer_namespace); + costmap->add_costmap_layer(predicted_object_costmap); + } else { + RCLCPP_ERROR(node.get_logger(), "Unknown layer type: %s", type.c_str()); + } + } + + return costmap; +} + +} // namespace booars_costmap_utils + +#endif diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml new file mode 100644 index 00000000..66e55ae9 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml @@ -0,0 +1,20 @@ + + + + booars_costmap_utils + 0.0.0 + The booars_costmap_utils pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + lanelet_costmap + multi_layered_costmap + predicted_object_costmap + rclcpp + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/CMakeLists.txt new file mode 100644 index 00000000..27b2ef37 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(lanelet_costmap) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_library(lanelet_costmap SHARED + src/lanelet_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/include/lanelet_costmap/lanelet_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/include/lanelet_costmap/lanelet_costmap.hpp new file mode 100644 index 00000000..b7185e9e --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/include/lanelet_costmap/lanelet_costmap.hpp @@ -0,0 +1,65 @@ +// Copyright 2024 Booars +// +// 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 LANELET_COSTMAP__LANELET_COSTMAP_HPP_ +#define LANELET_COSTMAP__LANELET_COSTMAP_HPP_ + +#include + +#include + +#include +#include +#include + +#include + +namespace lanelet_costmap +{ + +using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + +class LaneletCostmap : public multi_layered_costmap::CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + explicit LaneletCostmap(rclcpp::Node & node, const std::string & layer_namespace); + + static CostmapBase::SharedPtr create_costmap( + rclcpp::Node & node, const std::string & layer_namespace) + { + return std::make_shared(node, layer_namespace); + } + + bool is_ready() override; + bool is_occupied(const geometry_msgs::msg::PointStamped & point) override; + +private: + bool try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame); + void map_callback(const HADMapBin::SharedPtr msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr map_sub_; + + std::string map_frame_id_; + lanelet::LaneletMapPtr map_; + lanelet::ConstLanelets roads_; +}; +} // namespace lanelet_costmap + +#endif // LANELET_COSTMAP__LANELET_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/package.xml new file mode 100644 index 00000000..c695f51b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/package.xml @@ -0,0 +1,24 @@ + + + + lanelet_costmap + 0.0.0 + The lanelet_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_mapping_msgs + geometry_msgs + lanelet2_extension + multi_layered_costmap + rclcpp + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/src/lanelet_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/src/lanelet_costmap.cpp new file mode 100644 index 00000000..3ce26508 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/src/lanelet_costmap.cpp @@ -0,0 +1,84 @@ +// Copyright 2024 Booars +// +// 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 "lanelet_costmap/lanelet_costmap.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include + +namespace lanelet_costmap +{ +LaneletCostmap::LaneletCostmap(rclcpp::Node & node, const std::string & layer_namespace) +: tf_buffer_(node.get_clock()), tf_listener_(tf_buffer_) +{ + std::string map_topic = node.declare_parameter(layer_namespace + ".map_topic", "~/input/map"); + map_sub_ = node.create_subscription( + map_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(), + std::bind(&LaneletCostmap::map_callback, this, std::placeholders::_1)); +} + +bool LaneletCostmap::is_ready() +{ + return map_ != nullptr; +} + +bool LaneletCostmap::is_occupied(const geometry_msgs::msg::PointStamped & point) +{ + if (!is_ready()) return true; + + geometry_msgs::msg::Point transformed_point; + if (!try_transform_point(point, transformed_point, map_frame_id_)) { + return true; + } + + tier4_autoware_utils::Point2d point2d(transformed_point.x, transformed_point.y); + for (const auto & road : roads_) { + if (!lanelet::geometry::within(point2d, road.polygon2d().basicPolygon())) continue; + return false; + } + return true; +} + +bool LaneletCostmap::try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame) +{ + geometry_msgs::msg::PointStamped transformed_point_stamped; + try { + transformed_point_stamped = tf_buffer_.transform(point, target_frame); + } catch (tf2::TransformException & ex) { + return false; + } + + transformed_point = transformed_point_stamped.point; + return true; +} + +void LaneletCostmap::map_callback(const HADMapBin::SharedPtr msg) +{ + map_frame_id_ = msg->header.frame_id; + map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, map_); + const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map_); + roads_ = lanelet::utils::query::roadLanelets(all_lanelets); +} +} // namespace lanelet_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/CMakeLists.txt new file mode 100644 index 00000000..666ac04f --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(multi_layered_costmap) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_library(multi_layered_costmap SHARED + src/multi_layered_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/costmap_base.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/costmap_base.hpp new file mode 100644 index 00000000..12adc217 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/costmap_base.hpp @@ -0,0 +1,35 @@ +// Copyright 2024 Booars +// +// 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 MULTI_LAYERED_COSTMAP__COSTMAP_BASE_HPP_ +#define MULTI_LAYERED_COSTMAP__COSTMAP_BASE_HPP_ + +#include + +#include + +#include + +namespace multi_layered_costmap +{ +class CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + virtual bool is_ready() = 0; + virtual bool is_occupied(const geometry_msgs::msg::PointStamped & point) = 0; +}; +} // namespace multi_layered_costmap + +#endif // MULTI_LAYERED_COSTMAP__COSTMAP_BASE_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/multi_layered_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/multi_layered_costmap.hpp new file mode 100644 index 00000000..395ec450 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/multi_layered_costmap.hpp @@ -0,0 +1,37 @@ +// Copyright 2024 Booars +// +// 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 MULTI_LAYERED_COSTMAP__MULTI_LAYERED_COSTMAP_HPP_ +#define MULTI_LAYERED_COSTMAP__MULTI_LAYERED_COSTMAP_HPP_ + +#include "multi_layered_costmap/costmap_base.hpp" + +#include + +namespace multi_layered_costmap +{ +class MultiLayeredCostmap +{ +public: + using SharedPtr = std::shared_ptr; + void add_costmap_layer(CostmapBase::SharedPtr costmap_layer); + bool is_ready(); + bool is_occupied(const geometry_msgs::msg::PointStamped & point); + +private: + std::vector costmap_layers_; +}; +} // namespace multi_layered_costmap + +#endif // MULTI_LAYERED_COSTMAP__MULTI_LAYERED_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/package.xml new file mode 100644 index 00000000..d5ee5091 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/package.xml @@ -0,0 +1,18 @@ + + + + multi_layered_costmap + 0.0.0 + The multi_layered_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + rclcpp + geometry_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/src/multi_layered_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/src/multi_layered_costmap.cpp new file mode 100644 index 00000000..10ed829d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/src/multi_layered_costmap.cpp @@ -0,0 +1,40 @@ +// Copyright 2024 Booars +// +// 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 "multi_layered_costmap/multi_layered_costmap.hpp" + +namespace multi_layered_costmap +{ +void MultiLayeredCostmap::add_costmap_layer(CostmapBase::SharedPtr costmap_layer) +{ + costmap_layers_.push_back(costmap_layer); +} + +bool MultiLayeredCostmap::is_ready() +{ + for (const auto & costmap_layer : costmap_layers_) { + if (!costmap_layer->is_ready()) return false; + } + return true; +} + +bool MultiLayeredCostmap::is_occupied(const geometry_msgs::msg::PointStamped & point) +{ + for (const auto & costmap_layer : costmap_layers_) { + if (!costmap_layer->is_occupied(point)) continue; + return true; + } + return false; +} +} // namespace multi_layered_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/CMakeLists.txt new file mode 100644 index 00000000..db44765b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(predicted_object_costmap) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_library(predicted_object_costmap SHARED + src/predicted_object_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/include/predicted_object_costmap/predicted_object_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/include/predicted_object_costmap/predicted_object_costmap.hpp new file mode 100644 index 00000000..e2c161c7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/include/predicted_object_costmap/predicted_object_costmap.hpp @@ -0,0 +1,65 @@ +// Copyright 2024 Booars +// +// 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 PREDICTED_OBJECT_COSTMAP__PREDICTED_OBJECT_COSTMAP_HPP_ +#define PREDICTED_OBJECT_COSTMAP__PREDICTED_OBJECT_COSTMAP_HPP_ + +#include + +#include + +#include +#include + +#include + +namespace predicted_object_costmap +{ + +using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + +class PredictedObjectCostmap : public multi_layered_costmap::CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + explicit PredictedObjectCostmap(rclcpp::Node & node, const std::string & layer_namespace); + static multi_layered_costmap::CostmapBase::SharedPtr create_costmap( + rclcpp::Node & node, const std::string & layer_namespace) + { + return std::make_shared(node, layer_namespace); + } + + bool is_ready() override; + bool is_occupied(const geometry_msgs::msg::PointStamped & point) override; + +private: + bool try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame); + bool is_intersected(const PredictedObject & object, const geometry_msgs::msg::Point & point); + + void objects_callback(const PredictedObjects::SharedPtr msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr objects_sub_; + + PredictedObjects::SharedPtr objects_; + double distance_threshold_; +}; +} // namespace predicted_object_costmap + +#endif // PREDICTED_OBJECT_COSTMAP__PREDICTED_OBJECT_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/package.xml new file mode 100644 index 00000000..541840d1 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/package.xml @@ -0,0 +1,21 @@ + + + + predicted_object_costmap + 0.0.0 + The predicted_object_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + multi_layered_costmap + rclcpp + tf2_geometry_msgs + tf2_ros + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/src/predicted_object_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/src/predicted_object_costmap.cpp new file mode 100644 index 00000000..04c01ded --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/src/predicted_object_costmap.cpp @@ -0,0 +1,95 @@ +// Copyright 2024 Booars +// +// 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 "predicted_object_costmap/predicted_object_costmap.hpp" + +#include + +#include + +namespace predicted_object_costmap +{ +PredictedObjectCostmap::PredictedObjectCostmap( + rclcpp::Node & node, const std::string & layer_namespace) +: tf_buffer_(node.get_clock()), tf_listener_(tf_buffer_) +{ + std::string objects_topic = + node.declare_parameter(layer_namespace + ".predicted_objects_topic", "~/input/objects"); + objects_sub_ = node.create_subscription( + objects_topic, 1, + std::bind(&PredictedObjectCostmap::objects_callback, this, std::placeholders::_1)); + + distance_threshold_ = node.declare_parameter(layer_namespace + ".distance_threshold", 0.0); +} + +bool PredictedObjectCostmap::is_ready() +{ + return objects_ != nullptr; +} + +bool PredictedObjectCostmap::is_occupied(const geometry_msgs::msg::PointStamped & point) +{ + if (!is_ready()) return true; + + geometry_msgs::msg::Point transformed_point; + if (!try_transform_point(point, transformed_point, objects_->header.frame_id)) { + return true; + } + + for (const auto & object : objects_->objects) { + if (!is_intersected(object, transformed_point)) continue; + return true; + } + return false; +} + +bool PredictedObjectCostmap::try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame) +{ + geometry_msgs::msg::PointStamped transformed_point_stamped; + try { + transformed_point_stamped = tf_buffer_.transform(point, target_frame); + } catch (tf2::TransformException & ex) { + return false; + } + + transformed_point = transformed_point_stamped.point; + return true; +} + +bool PredictedObjectCostmap::is_intersected( + const PredictedObject & object, const geometry_msgs::msg::Point & point) +{ + if (object.shape.type != object.shape.CYLINDER) return false; + + const geometry_msgs::msg::Point center = + object.kinematics.initial_pose_with_covariance.pose.position; + const double radius = + std::max(object.shape.dimensions.x, object.shape.dimensions.y) * 0.5 + distance_threshold_; + const double sqr_radius = radius * radius; + + const double dx = point.x - center.x; + const double dy = point.y - center.y; + const double sqr_distance = dx * dx + dy * dy; + + return sqr_distance < sqr_radius; +} + +void PredictedObjectCostmap::objects_callback(const PredictedObjects::SharedPtr msg) +{ + objects_ = msg; +} + +} // namespace predicted_object_costmap From bbc8d87122498a732f384deb60f12d04e969ed09 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 30 Aug 2024 02:41:45 +0900 Subject: [PATCH 14/48] launch costmap_generator Signed-off-by: Autumn60 --- .../planning/costmap_generator.param.yaml | 18 ++++++++++++++++++ .../launch/components/planning.launch.xml | 11 +++++++++++ .../launch/costmap_generator.launch.xml | 3 +++ 3 files changed, 32 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml new file mode 100644 index 00000000..761d16e7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + update_rate: 10.0 + map_frame_id: "map" + costmap_center_frame_id: "base_link" + costmap_width: 50.0 + costmap_resolution: 0.2 + multi_layered_costmap: + layers: + - "lanelet_layer" + - "predicted_object_layer" + lanelet_layer: + type: "lanelet" + map_topic: "/map/vector_map" + predicted_object_layer: + type: "predicted_object" + predicted_objects_topic: "/perception/object_recognition/objects" + distance_threshold: 1.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index 2c8c989f..483d4cd0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -100,5 +100,16 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml index 9501d94e..0dea8897 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml @@ -1,7 +1,10 @@ + + + \ No newline at end of file From 9f75c9492ac02db832357b9885e2a86b94772918 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 30 Aug 2024 02:41:56 +0900 Subject: [PATCH 15/48] add costmap to rviz Signed-off-by: Autumn60 --- .../config/autoware.rviz | 486 ++++++++++++++---- 1 file changed, 399 insertions(+), 87 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index 1539328f..2d248d76 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -3,9 +3,11 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Planning1 Splitter Ratio: 0.557669460773468 - Tree Height: 397 + Tree Height: 242 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -13,7 +15,8 @@ Panels: Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views - Expanded: ~ + Expanded: + - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel @@ -39,7 +42,8 @@ Visualization Manager: Show Arrows: true Show Axes: true Show Names: true - Tree: {} + Tree: + {} Update Interval: 0 Value: false - Alpha: 0.5 @@ -64,9 +68,12 @@ Visualization Manager: Displays: - Class: rviz_plugins/SteeringAngle Enabled: true + Left: 85 + Length: 171 Name: SteeringAngle Scale: 17 Text Color: 25; 255; 240 + Top: 85 Topic: Depth: 5 Durability Policy: Volatile @@ -74,12 +81,15 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/steering_status Value: true + Value Scale: 0.14999249577522278 Value height offset: 0 - Class: rviz_plugins/ConsoleMeter Enabled: true + Left: 341 + Length: 171 Name: ConsoleMeter - Scale: 3 Text Color: 25; 255; 240 + Top: 85 Topic: Depth: 5 Durability Policy: Volatile @@ -87,8 +97,9 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/velocity_status Value: true + Value Scale: 0.14999249577522278 Value height offset: 0 - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Class: rviz_plugins/VelocityHistory Color Border Vel Max: 3 Constant Color: @@ -157,8 +168,8 @@ Visualization Manager: Max Alpha: 0.5 Max Range: 100 Max Wave Alpha: 0.5 - Min Alpha: 0.01 - Min Wave Alpha: 0.01 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 Name: PolarGridDisplay Reference Frame: base_link Value: true @@ -166,18 +177,20 @@ Visualization Manager: Wave Velocity: 40 - Class: rviz_plugins/MaxVelocity Enabled: true + Left: 397 + Length: 64 Name: MaxVelocity Text Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/current_max_velocity + Top: 187 + Topic: /planning/scenario_planning/current_max_velocity Value: true + Value Scale: 0.25 - Class: rviz_plugins/TurnSignal Enabled: true + Height: 171 + Left: 131 Name: TurnSignal + Top: 233 Topic: Depth: 5 Durability Policy: Volatile @@ -185,6 +198,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/turn_indicators_status Value: true + Width: 341 Enabled: true Name: Vehicle Enabled: true @@ -213,11 +227,12 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 1 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Transient Local + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /map/pointcloud_map @@ -248,7 +263,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.4 + - Alpha: 0.4000000059604645 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 5 @@ -270,18 +285,19 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 1 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /sensing/lidar/concatenated/pointcloud Use Fixed Frame: false Use rainbow: true Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Class: rviz_default_plugins/Polygon Color: 25; 255; 0 Enabled: false @@ -289,6 +305,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /sensing/lidar/crop_box_filter/crop_box_polygon @@ -327,6 +344,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /sensing/gnss/pose_with_covariance @@ -339,7 +357,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/PoseWithCovariance @@ -369,11 +387,12 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/initial_pose_with_covariance - Value: true - - Alpha: 0.999 + Value: false + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/PoseWithCovariance @@ -403,27 +422,29 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/pose_with_covariance - Value: true + Value: false - Buffer Size: 200 Class: rviz_plugins::PoseHistory Enabled: false Line: + Alpha: 0.9990000128746033 Color: 170; 255; 127 Value: true Width: 0.10000000149011612 - Alpha: 0.999 Name: PoseHistory Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/pose - Value: true - - Alpha: 0.999 + Value: false + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -450,13 +471,14 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /localization/util/downsample/pointcloud Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -483,6 +505,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/points_aligned @@ -492,7 +515,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MonteCarloInitialPose - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -508,14 +532,15 @@ Visualization Manager: Class: rviz_plugins::PoseHistory Enabled: true Line: + Alpha: 0.9990000128746033 Color: 0; 255; 255 Value: true Width: 0.10000000149011612 - Alpha: 0.999 Name: PoseHistory Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_twist_fusion_filter/pose @@ -528,7 +553,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 15 @@ -550,11 +575,12 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 3 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /perception/obstacle_segmentation/pointcloud @@ -568,16 +594,16 @@ Visualization Manager: - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: true Display Predicted Path Confidence: true @@ -585,21 +611,23 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: DetectedObjects - Namespaces: {} + Namespaces: + {} PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -608,24 +636,25 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/detection/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal Enabled: false Name: Detection - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: true Display Predicted Path Confidence: true @@ -633,21 +662,23 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: TrackedObjects - Namespaces: {} + Namespaces: + {} PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -656,24 +687,25 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/tracking/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal Enabled: false Name: Tracking - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: false Display Predicted Path Confidence: true @@ -681,21 +713,28 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: PredictedObjects - Namespaces: {} + Namespaces: + acceleration: true + label: true + shape: true + twist: true + uuid: true + velocity: true PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -704,20 +743,22 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Maneuver - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort Value: /perception/object_recognition/prediction/maneuver - Value: true + Value: false Enabled: true Name: Prediction Enabled: true @@ -741,7 +782,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MapBasedDetectionResult - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -798,7 +840,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/mission_planning/route_marker Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.30000001192092896 Class: rviz_default_plugins/Pose @@ -813,6 +855,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/mission_planning/echo_back_goal_pose @@ -828,18 +871,36 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/trajectory Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Scale: 0.30000001192092896 @@ -855,16 +916,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/path Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.4000000059604645 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.4000000059604645 Color: 0; 0; 0 @@ -878,16 +962,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/lane_change Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -901,16 +1008,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/ext_request_lane_change_right Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -924,16 +1054,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/ext_request_lane_change_left Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -947,16 +1100,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/avoidance Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -970,16 +1146,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/pull_out Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -993,16 +1192,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/pull_over Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -1018,7 +1240,6 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound @@ -1414,18 +1635,36 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/trajectory Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Scale: 0.30000001192092896 @@ -1608,6 +1847,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid @@ -1619,7 +1859,7 @@ Visualization Manager: Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates Use Timestamp: false Value: false - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Arrow Length: 0.30000001192092896 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 @@ -1635,11 +1875,12 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Arrow Length: 0.5 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 @@ -1655,6 +1896,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array @@ -1684,7 +1926,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: PlanningErrorMarker - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1694,6 +1937,31 @@ Visualization Manager: Value: true Enabled: false Name: Diagnostic + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/costmap/costmap_generator/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/costmap/costmap_generator/costmap_updates + Use Timestamp: false + Value: true + Enabled: true + Name: Costmap Enabled: true Name: Planning - Class: rviz_common/Group @@ -1705,16 +1973,34 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /control/trajectory_follower/lateral/predicted_trajectory Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 1 Color: 255; 255; 255 Constant Color: true Value: true Width: 0.05000000074505806 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 1 Color: 0; 0; 0 @@ -1724,7 +2010,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Debug/MPC - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1735,7 +2022,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Debug/PurePursuit - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1797,15 +2085,15 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose - Theta std deviation: 0.2617993950843811 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 @@ -1813,26 +2101,47 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/mission_planning/goal - - Class: rviz_plugins/PedestrianInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 0 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 1 Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/CarInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 3 + W vehicle width: 1.7999999523162842 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 0 Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/BusInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 0 + W vehicle width: 2.5 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 0 @@ -1845,6 +2154,9 @@ Visualization Manager: Z position: 0 - Class: rviz_plugins/DeleteAllObjectsTool Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: @@ -1858,7 +2170,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 10 + Scale: 10.328941345214844 Target Frame: viewer Value: TopDownOrtho (rviz_default_plugins) X: 0 @@ -1908,12 +2220,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1016 + Height: 1376 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -1922,6 +2234,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 + Width: 3370 X: 70 Y: 27 From d73cca455ccb5218db638ebb1f1dd3b29362fa3b Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 06:20:07 +0900 Subject: [PATCH 16/48] MPPI operation check --- aichallenge/pkill_all.sh | 0 .../launch/components/planning.launch.xml | 12 + .../config/mppi_controller.param.yaml | 26 + .../mppi_controller/mppi_controller/MPPI.py | 463 ++++++++++++++++++ .../mppi_controller/__init__.py | 0 .../__pycache__/MPPI.cpython-310.pyc | Bin 0 -> 10093 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 165 bytes .../cost_map_tensor.cpython-310.pyc | Bin 0 -> 1831 bytes .../mppi_controller.cpython-310.pyc | Bin 0 -> 7808 bytes .../mppi_controller_node.cpython-310.pyc | Bin 0 -> 6746 bytes .../mppi_controller/cost_map_tensor.py | 49 ++ .../mppi_controller/mppi_controller.py | 244 +++++++++ .../mppi_controller/mppi_controller_node.py | 250 ++++++++++ .../mppi_controller/package.xml | 16 + .../mppi_controller/resource/mppi_controller | 0 .../mppi_controller/setup.cfg | 4 + .../mppi_controller/setup.py | 33 ++ .../config/autoware.rviz | 75 ++- .../launch/aichallenge_system.launch.xml | 2 +- 19 files changed, 1160 insertions(+), 14 deletions(-) mode change 100644 => 100755 aichallenge/pkill_all.sh create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller_node.cpython-310.pyc create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py diff --git a/aichallenge/pkill_all.sh b/aichallenge/pkill_all.sh old mode 100644 new mode 100755 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index 483d4cd0..05e295a0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -111,5 +111,17 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml new file mode 100644 index 00000000..96d2990a --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + # mppi + horizon : 25 + num_samples : 3000 + u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad) + u_max : [2.0, 0.25] + sigmas : [0.5, 0.1] # sample range + lambda : 1.0 + auto_lambda : false + # reference path + DL : 0.1 + loolahead_distance : 3.0 + reference_path_interval : 0.85 + # cost weights + Qc : 2.0 + Ql : 3.0 + Qv : 2.0 + Qo : 10000.0 + Qin : 0.01 + Qdin : 0.5 + # model param + delta_t : 0.1 + vehicle_L : 1.0 + V_MAX : 8.0 + diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py new file mode 100644 index 00000000..18ca6044 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -0,0 +1,463 @@ +""" +Kohei Honda, 2023. +""" + +from __future__ import annotations + +from typing import Callable, Tuple, Dict +import torch +import torch.nn as nn +from torch.distributions.multivariate_normal import MultivariateNormal + + +class MPPI(nn.Module): + """ + Model Predictive Path Integral Control, + J. Williams et al., T-RO, 2017. + """ + + def __init__( + self, + horizon: int, + num_samples: int, + dim_state: int, + dim_control: int, + dynamics: Callable[[torch.Tensor, torch.Tensor], torch.Tensor], + cost_func: Callable[[torch.Tensor, torch.Tensor, Dict], torch.Tensor], + u_min: torch.Tensor, + u_max: torch.Tensor, + sigmas: torch.Tensor, + lambda_: float, + auto_lambda: bool = False, + exploration: float = 0.0, + use_sg_filter: bool = False, + sg_window_size: int = 5, + sg_poly_order: int = 3, + device=torch.device("cuda"), + dtype=torch.float32, + seed: int = 42, + ) -> None: + """ + :param horizon: Predictive horizon length. + :param predictive_interval: Predictive interval (seconds). + :param delta: predictive horizon step size (seconds). + :param num_samples: Number of samples. + :param dim_state: Dimension of state. + :param dim_control: Dimension of control. + :param dynamics: Dynamics model. + :param cost_func: Cost function. + :param u_min: Minimum control. + :param u_max: Maximum control. + :param sigmas: Noise standard deviation for each control dimension. + :param lambda_: temperature parameter. + :param exploration: Exploration rate when sampling. + :param use_sg_filter: Use Savitzky-Golay filter. + :param sg_window_size: Window size for Savitzky-Golay filter. larger is smoother. Must be odd. + :param sg_poly_order: Polynomial order for Savitzky-Golay filter. Smaller is smoother. + :param device: Device to run the solver. + :param dtype: Data type to run the solver. + :param seed: Seed for torch. + """ + + super().__init__() + + # torch seed + torch.manual_seed(seed) + + # check dimensions + assert u_min.shape == (dim_control,) + assert u_max.shape == (dim_control,) + assert sigmas.shape == (dim_control,) + # assert num_samples % batch_size == 0 and num_samples >= batch_size + + # device and dtype + if torch.cuda.is_available() and device == torch.device("cuda"): + self._device = torch.device("cuda") + else: + self._device = torch.device("cpu") + print(f"Device: {self._device}") + self._dtype = dtype + + # set parameters + self._horizon = horizon + self._num_samples = num_samples + self._dim_state = dim_state + self._dim_control = dim_control + self._dynamics = dynamics + self._cost_func = cost_func + self._u_min = u_min.clone().detach().to(self._device, self._dtype) + self._u_max = u_max.clone().detach().to(self._device, self._dtype) + self._sigmas = sigmas.clone().detach().to(self._device, self._dtype) + self._lambda = lambda_ + self._exploration = exploration + self._use_sg_filter = use_sg_filter + self._sg_window_size = sg_window_size + self._sg_poly_order = sg_poly_order + + # noise distribution + self._covariance = torch.zeros( + self._horizon, + self._dim_control, + self._dim_control, + device=self._device, + dtype=self._dtype, + ) + self._covariance[:, :, :] = torch.diag(sigmas**2).to(self._device, self._dtype) + self._inv_covariance = torch.zeros_like( + self._covariance, device=self._device, dtype=self._dtype + ) + for t in range(1, self._horizon): + self._inv_covariance[t] = torch.inverse(self._covariance[t]) + + zero_mean = torch.zeros(dim_control, device=self._device, dtype=self._dtype) + self._noise_distribution = MultivariateNormal( + loc=zero_mean, covariance_matrix=self._covariance + ) + + self._sample_shape = torch.Size([self._num_samples]) + + # sampling with reparameting trick + self._action_noises = self._noise_distribution.rsample( + sample_shape=self._sample_shape + ) + + zero_mean_seq = torch.zeros( + self._horizon, self._dim_control, device=self._device, dtype=self._dtype + ) + self._perturbed_action_seqs = torch.clamp( + zero_mean_seq + self._action_noises, self._u_min, self._u_max + ) + + self._previous_action_seq = zero_mean_seq + + # init satitzky-golay filter + self._coeffs = self._savitzky_golay_coeffs( + self._sg_window_size, self._sg_poly_order + ) + self._actions_history_for_sg = torch.zeros( + self._horizon - 1, self._dim_control, device=self._device, dtype=self._dtype + ) # previous inputted actions for sg filter + + # inner variables + self._state_seq_batch = torch.zeros( + self._num_samples, + self._horizon + 1, + self._dim_state, + device=self._device, + dtype=self._dtype, + ) + self._weights = torch.zeros( + self._num_samples, device=self._device, dtype=self._dtype + ) + self._optimal_state_seq = torch.zeros( + self._horizon + 1, self._dim_state, device=self._device, dtype=self._dtype + ) + + # auto lambda tuning + self._auto_lambda = auto_lambda + if auto_lambda: + self.log_tempature = torch.nn.Parameter( + torch.log( + torch.tensor([self._lambda], device=self._device, dtype=self._dtype) + ) + ) + self.optimizer = torch.optim.Adam([self.log_tempature], lr=1e-2) + + def reset(self): + """ + Reset the previous action sequence. + """ + self._previous_action_seq = torch.zeros( + self._horizon, self._dim_control, device=self._device, dtype=self._dtype + ) + self._actions_history_for_sg = torch.zeros( + self._horizon - 1, self._dim_control, device=self._device, dtype=self._dtype + ) # previous inputted actions for sg filter + + def forward( + self, state: torch.Tensor, info: Dict = {} + ) -> Tuple[torch.Tensor, torch.Tensor]: + """ + Solve the optimal control problem. + Args: + state (torch.Tensor): Current state. + Returns: + Tuple[torch.Tensor, torch.Tensor]: Tuple of predictive control and state sequence. + """ + assert state.shape == (self._dim_state,) + + if not torch.is_tensor(state): + state = torch.tensor(state, device=self._device, dtype=self._dtype) + else: + if state.device != self._device or state.dtype != self._dtype: + state = state.to(self._device, self._dtype) + + mean_action_seq = self._previous_action_seq.clone().detach() + + # random sampling with reparametrization trick + self._action_noises = self._noise_distribution.rsample( + sample_shape=self._sample_shape + ) + + # noise injection with exploration + threshold = int(self._num_samples * (1 - self._exploration)) + inherited_samples = mean_action_seq + self._action_noises[:threshold] + self._perturbed_action_seqs = torch.cat( + [inherited_samples, self._action_noises[threshold:]] + ) + + # clamp actions + self._perturbed_action_seqs = torch.clamp( + self._perturbed_action_seqs, self._u_min, self._u_max + ) + + # rollout samples in parallel + self._state_seq_batch[:, 0, :] = state.repeat(self._num_samples, 1) + + for t in range(self._horizon): + self._state_seq_batch[:, t + 1, :] = self._dynamics( + self._state_seq_batch[:, t, :], + self._perturbed_action_seqs[:, t, :], + ) + + # compute sample costs + costs = torch.zeros( + self._num_samples, self._horizon, device=self._device, dtype=self._dtype + ) + action_costs = torch.zeros( + self._num_samples, self._horizon, device=self._device, dtype=self._dtype + ) + initial_state = self._state_seq_batch[:, 0, :] + for t in range(self._horizon): + prev_index = t - 1 if t > 0 else 0 + prev_state = self._state_seq_batch[:, prev_index, :] + prev_action = self._perturbed_action_seqs[:, prev_index, :] + # info update + info["prev_state"] = prev_state + info["prev_action"] = prev_action + info["initial_state"] = initial_state + info["t"] = t + costs[:, t] = self._cost_func( + self._state_seq_batch[:, t, :], + self._perturbed_action_seqs[:, t, :], + info, + ) + action_costs[:, t] = ( + mean_action_seq[t] + @ self._inv_covariance[t] + @ self._perturbed_action_seqs[:, t].T + ) + + prev_state = self._state_seq_batch[:, -2, :] + info["prev_state"] = prev_state + zero_action = torch.zeros( + self._num_samples, + self._dim_control, + device=self._device, + dtype=self._dtype, + ) + terminal_costs = self._cost_func( + self._state_seq_batch[:, -1, :], zero_action, info + ) + + # In the original paper, the action cost is added to consider KL div. penalty, + # but it is easier to tune without it + costs = ( + torch.sum(costs, dim=1) + + terminal_costs + # + torch.sum(self._lambda * action_costs, dim=1) + ) + + # calculate weights + self._weights = torch.softmax(-costs / self._lambda, dim=0) + + # find optimal control by weighted average + optimal_action_seq = torch.sum( + self._weights.view(self._num_samples, 1, 1) * self._perturbed_action_seqs, + dim=0, + ) + + mean_action_seq = optimal_action_seq + + # auto-tune temperature parameter + # Refer E step of MPO algorithm: + # https://arxiv.org/pdf/1806.06920 + if self._auto_lambda: + for _ in range(1): + self.optimizer.zero_grad() + tempature = torch.nn.functional.softplus(self.log_tempature) + cost_logsumexp = torch.logsumexp(-costs / tempature, dim=0) + epsilon = 0.1 # tolerance hyperparameter for KL divergence + loss = tempature * (epsilon + torch.mean(cost_logsumexp)) + loss.backward() + self.optimizer.step() + self._lambda = torch.exp(self.log_tempature).item() + + # calculate new covariance + # https://arxiv.org/pdf/2104.00241 + # covariance = torch.sum( + # self._weights.view(self._num_samples, 1, 1) + # * (self._perturbed_action_seqs - optimal_action_seq) ** 2, + # dim=0, + # ) # T x dim_control + + # small_cov = 1e-6 * torch.eye( + # self._dim_control, device=self._device, dtype=self._dtype + # ) + # self._covariance = torch.diag_embed(covariance) + small_cov + + # for t in range(1, self._horizon): + # self._inv_covariance[t] = torch.inverse(self._covariance[t]) + # zero_mean = torch.zeros(self._dim_control, device=self._device, dtype=self._dtype) + # self._noise_distribution = MultivariateNormal( + # loc=zero_mean, covariance_matrix=self._covariance + # ) + + if self._use_sg_filter: + # apply savitzky-golay filter to N-1 previous action history + N optimal action seq + prolonged_action_seq = torch.cat( + [ + self._actions_history_for_sg, + optimal_action_seq, + ], + dim=0, + ) + + # appply sg filter for each control dimension + filtered_action_seq = torch.zeros_like( + prolonged_action_seq, device=self._device, dtype=self._dtype + ) + for i in range(self._dim_control): + filtered_action_seq[:, i] = self._apply_savitzky_golay( + prolonged_action_seq[:, i], self._coeffs + ) + + # use only N step optimal action seq + optimal_action_seq = filtered_action_seq[-self._horizon :] + + # predivtive state seq + expanded_optimal_action_seq = optimal_action_seq.repeat(1, 1, 1) + optimal_state_seq = self._states_prediction(state, expanded_optimal_action_seq) + + # update previous actions + self._previous_action_seq = optimal_action_seq + + # stuck previous actions for sg filter + optimal_action = optimal_action_seq[0] + self._actions_history_for_sg = torch.cat( + [self._actions_history_for_sg[1:], optimal_action.view(1, -1)] + ) + + return optimal_action_seq, optimal_state_seq + + def get_top_samples(self, num_samples: int) -> Tuple[torch.Tensor, torch.Tensor]: + """ + Get top samples. + Args: + num_samples (int): Number of state samples to get. + Returns: + Tuple[torch.Tensor, torch.Tensor]: Tuple of top samples and their weights. + """ + assert num_samples <= self._num_samples + + # large weights are better + top_indices = torch.topk(self._weights, num_samples).indices + + top_samples = self._state_seq_batch[top_indices] + top_weights = self._weights[top_indices] + + top_samples = top_samples[torch.argsort(top_weights, descending=True)] + top_weights = top_weights[torch.argsort(top_weights, descending=True)] + + return top_samples, top_weights + + def get_samples_from_posterior( + self, optimal_solution: torch.Tensor, state: torch.Tensor, num_samples: int + ) -> Tuple[torch.Tensor]: + assert num_samples <= self._num_samples + + # posterior distribution of MPPI + # covaraince is the same as noise distribution + posterior_distribution = MultivariateNormal( + loc=optimal_solution, covariance_matrix=self._covariance + ) + + # sampling control input sequence from posterior + samples = posterior_distribution.sample(sample_shape=torch.Size([num_samples])) + + # get state sequence from sampled control input sequence + predictive_states = self._states_prediction(state, samples) + + return samples, predictive_states + + def _states_prediction( + self, state: torch.Tensor, action_seqs: torch.Tensor + ) -> torch.Tensor: + state_seqs = torch.zeros( + action_seqs.shape[0], + self._horizon + 1, + self._dim_state, + device=self._device, + dtype=self._dtype, + ) + state_seqs[:, 0, :] = state + # expanded_optimal_action_seq = action_seq.repeat(1, 1, 1) + for t in range(self._horizon): + state_seqs[:, t + 1, :] = self._dynamics( + state_seqs[:, t, :], action_seqs[:, t, :] + ) + return state_seqs + + def _savitzky_golay_coeffs(self, window_size: int, poly_order: int) -> torch.Tensor: + """ + Compute the Savitzky-Golay filter coefficients using PyTorch. + + Parameters: + - window_size: The size of the window (must be odd). + - poly_order: The order of the polynomial to fit. + + Returns: + - coeffs: The filter coefficients as a PyTorch tensor. + """ + # Ensure the window size is odd and greater than the polynomial order + if window_size % 2 == 0 or window_size <= poly_order: + raise ValueError("window_size must be odd and greater than poly_order.") + + # Generate the Vandermonde matrix of powers for the polynomial fit + half_window = (window_size - 1) // 2 + indices = torch.arange( + -half_window, half_window + 1, dtype=self._dtype, device=self._device + ) + A = torch.vander(indices, N=poly_order + 1, increasing=True) + + # Compute the pseudo-inverse of the matrix + pseudo_inverse = torch.linalg.pinv(A) + + # The filter coefficients are given by the first row of the pseudo-inverse + coeffs = pseudo_inverse[0] + + return coeffs + + def _apply_savitzky_golay( + self, y: torch.Tensor, coeffs: torch.Tensor + ) -> torch.Tensor: + """ + Apply the Savitzky-Golay filter to a 1D signal using the provided coefficients. + + Parameters: + - y: The input signal as a PyTorch tensor. + - coeffs: The filter coefficients as a PyTorch tensor. + + Returns: + - y_filtered: The filtered signal. + """ + # Pad the signal at both ends to handle the borders + pad_size = len(coeffs) // 2 + y_padded = torch.cat([y[:pad_size].flip(0), y, y[-pad_size:].flip(0)]) + + # Apply convolution + y_filtered = torch.conv1d( + y_padded.view(1, 1, -1), coeffs.view(1, 1, -1), padding="valid" + ) + + return y_filtered.view(-1) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..624e444edbe033ca6540a9d6462d8a7a15a6e5de GIT binary patch literal 10093 zcmbtaON<*=cCA0Z&Hjk$pZqDwmMz*+%aZ?XjctwXamE8_6ic2-+Hq=1d_{J#i&fOC zVoT~NW`an@APEF`6(AV|VIx2gXOZt}Hpw~&5Cqu_@Q_U~D=o6iN>1E4_Z5p|cT0+q zu))js|J--qz31N7(acO)!Dr^)zqHnJndy&#%)5%B#v4d5WkX>q(*m`tZD=Z` z^+4|$8%EdMFuT@ zRi#nfsh7VOZF;`_D^bYY)Asq7&cD1?zE5jusG{PAVU)OuABAy47sdD7AaJh-o-nWV zd-yTl_nXOOG+emU4-$Xd<-VJEmm}VFgC>nJ{V8S{HNK04r);R$m&Vi$ooS4|1~WZl zXfkV8^^6Wy+bXiReyX%oX0zjWXRx;u?3ugDM$xCT3+xnY+%-2!>|5*w_TpV*qs-oB zYwRpi74{B0$Ic^FW$&_A*=tD6u#4#gwgCX_x=l%ENn*H--$w@`(Yv~VZZCdZWnBgMTz;yfbpKF zP_miLEsE@R=yv^P{NR=RC_eo(iFk8!?V1qx4UjE3W3|b&Rj>Btsru-KewMmPF5(4>roVlig&9Q zL|lrOnCZu!6Stj~A0!?ZH6(BPA&YK0vA^Sq8RUCWaNCJEL$wim9upS#l0FZGaXAXT z=8>G;_+7-egfDom#x>AHxuXo#T}4$AZK$UUyQ-?Bg^rOLEp+A(^I8W{F(B{+8vs49;M}! z>ME%QT1`>!%%lcrEj2-BQwwx1Eez*(mEi)uM`#3m%e&O0o)#Zbb2KeIqGmBIqvuju z1zk>OKv&Wl=xRC(dJOw|0^?WkMXCJC`IvE!R`eux^i*16<`HFA-M^a7lh7*G=h`%F z=@-D`&eIs_NqqB>XrFd+oE1=3xude-7zqJI{kY$4d98{Oe) zFU|SVWb8xzNG{H$I2q@BTKPidpQG)|!&k6=NL}YO%&^oZzKzG*&A9P8YTxL*iJI5j zDl4;!p2uEGvL^Htp!VPd43BGtFvbGh5KM+#|uS_zKI?7$1#$!4b6rn>aT>e7K0 z^Z2$K?CYKv+NWX>iSl6N2y3m<0ZL&Qn+lRIu>D~kFN2P~*j1)AQk5YD*?8>2rV@SFtFY>EI09@6>-~ho16JW zHtbKvrb8!;bJGj6Q}e_2p}zZ! z!@B(m)_&F9_LH5h+h;zE0{6C^*^NUT_nVP*SZ%T?nNZPIA3lm5xV#NP@?$&hMp3ef z?bw(4IHv2K9WnN>X(rYQ7XX$jjJiHxgS1zYtB;uKDzH*;V6KA~la8n9L8jy{Y`o3; zp^atPaTIJngx4&&-J|wy;@Xt_23^RauG?2ZsR=7v znK0#q_cb)Sd|w4cogPF@G2e_NDh!(*;6cLuTVJZ8n%zz(-gJ9j`^WXa{P5p*?!E&U zsRg|8mN4T!#7Pt#s1!eO03qaJM5XJ7eK&BV?KfrTY*|%5cHC{(m)Mj)M=LEj`O12n z;Faab6=KjFbyb}y)gj8x1aehndUVl5$;m+%8jWwQFq;8D2QeVQZ4p`$VGem$vtK}= z0-#HVg>u|eb*3(Xs5$$@TFesunFc*IDwxQXFn2s2#iD{`%8v zhuhOlOzwtlPZUt>@z~=QG3baBk_y09W1PzMK3#rcT$NaBghR9Q5e1$d7uM@a6_xdj zi3Px`V$s2A;qG7eSl%G^Zp5PQK&5Z{Q6Gns^+PdQ$D@nuwCUnvpldVYH|iN%5w^%5U9Y0^eiooUFag`p^2Au~tH ziG;H)tVDulF@ualN=&LSValxA z_s_b1bCYxj4)^TMh;PL`x9Od|-uDA`w%hCZ;HY$2pj{5-$?sX~-R9>oEiF49f|Oq@ z>vQ^?I;SohHMOeEsg`Q#_>TToRrC>=sZz=|E9EFgKMV>0y~Au21cW12*B2kQ$=2Sze>ULC8ykG&Xf z9x2>hrtM6oZJbQsgLa#k1bz~;^9IP}Mv=Y|PkG-Pn*kC#C~f3rAhOrw`PyjvfT=%`H7Xi8Mh{YfI;6 zQgv9$+wN)u>Q^3C;Mi1ml>xDSM#AY?rb*a7m%vHtEJ(-c$g)yb?x+KU+@#JNgS+;* zwxTR40}~LqP7YINE;X1eZD&pwf%`?26TDx-cUc1aqgcfXau}BYeP;(jtbZ0)BsM{kldG1yVT0ttq@m^X1VFo=}}RxFDZEsI5fWnjy;8b3p4}1Ip2&y z;gL1OzeB5BQ3j=Cvd$3ew3xYN@PDH@mz7Xu<^axK-ZL%dt3Xe3@bZ;UmsjCnz=gY@ zz(Lel0ly{u7V&H0w}fA1NbcM>=0&Q3=Y^a{Ee#{+sU6q9=33zo#QAgPWQO+ zuag2epN)lPAaoX81J;do82msKXc;rwE4gBtoigB9wm3dg7hd7^W8HZ4?W zWhaj*kPE7TTPa(@sT`7P3F(3SkSrdpkJ=1o(^)CSr{JFbB00u%DM4$4IkajFU&ulZ zofkW&hi6c-wyO^2G3Hr0*L*(HV1dCMMtFc+;+H5qu>{E<)YJO#9E~8yI!~!D)Ej?- zHZNym4PV(sopTboSHI9WdP`^awRE;kvW-2Ev)g%HI=eZC^ueN(p%ZeYiv!$};Tvf# z8B25?81GF;^y275P}sQ1ixsCUEe&e7w)4xe+PzBXR;t~Rc^`WvJsu$r*_~+J zi@Y7Lk2{iwDpjI=YC1^OfV0)-+zXSecGCT08A&=Y!vCs5*KOH^9LOp6b!2vJ_*SDu zJjzI%fyT`&1tl+MM5F=y7Nw*;e4A32L4bSigB98m{kU7e`^^SZ2X* z&?uZ5&C-O^*te8y!d!1g0Tc6n2&WR$vOMT34;7di!GKs)M~GSG_%@A}$p+k4?-oyJ zjy{!EBt{%bNe##rfmw)2Hp)T{ibtbC429~5(gf<(q(d{|QNioQJ{%ii1W^nKk86fp zdmpkFM>1b~f1&D&V_+p@9_0_2cRnwZj3YoRYI#w%yo1=M04PHT^*7jO{Ldf@<+8S@ zE(4a@8N2`$YkAms_OrV(IVRG<3VuqKdmS+A*ebs2v}tq8bDfRkEj>fC=@ zRuKSmLDRmT+gnCD2egK2{g+{_s>WWmprM7fw_GrE>SKLvm_^K2#>{oJU%+@ZqKk%R z0QhQOYliieQ7U6aHNf8mwZ1oJ)^w~;FCxE;@}jX*mH2J6$tJnZ`G)`;eI*$CO(Z1P zQnu8%Ac2@V%uubm3m`V2w3(7RCc~u56aZ7Ab}T?6yc^xi>Wl*=1gZ?Fa`l~Gj5X(n zgmWW=-Lj{MiH1C^V5j_N`xKmPXxFI#90AgN6be7Iy<{@j|6AQoEm7)OKy#nlBX@8z z&o$`AGV|i57c#_^8ih=ANu_9DjIB(;z%|A#_F}25L;=Acyrm=!GrQ5`>WKS!OizK^dOKt{v(*YV1PI$24B8IH#Ajvbvx_ZnT}cl!XJ0#awh)(!>0E{pM>Pt=7a0~d_ z$^_&xe*t50vR63|$=TxH1HqGzl$FfY`Iw4-KWisFnlGt9Dn-f1#U2zKG#`BWeW8a! z&d&Kp0Qbz5ie~J4C7=F*hO;l5z;pG-C2HrX8SGpMFAY}mY zB-pPEi+L;D)KM#ps%7cvGJvtscqqk;d22Wq&!o~@y(}SwL=(~jZIq^bPl*Sl{gt3# zV*jOKCYziKyF97CMu8}gHFYd)(}81)>n%K(!;0JC*aEOEJ@xU zHkhI^9qTGj;#~XqPp`%#YEqzkNTe!L7Zs-A)s9}$v@Mm_5`|zlUhwexy{s^G6}k}G z4m<;(2jr9|MA{K=%=e4|X+jfkZg_JBC-Kq@2i7F*I4nT$Rq0fb&Mc$_0Mep+8oL}T z$oHaduMgEm!GMQ`bb(P3(ej%s54cLLefyiEg#Bwyp}`S_)NBkI2$K3_xrcy z#d!A@rehTL##c}7AsU6u<3ztf}%>fV*`c5|8aBB5yQivN`78oOhdaa=e z^|C1YVG~`5qm7~{|G*9U-UpmVJVUt_qIUjMf@OUiqI=iF#4gda>7M5d~8zYAUQOPfbVJ)YHj@8`hCS8W{O{3mpU9KAlOC5MPfJap67E3kGqHy8hptX_^1z1Vc|q(+ z-M}l;CS`4uOF@U0$3IoTe`Y2;S8bBc1wS=W2CISn5Yjf- zI}&x0RKUh zMDOhX9*Vae8VUdGP@XL9pbY*VI{utwkYa?@+8P402-EJBjh)p@_8#WQG)`oJZ0XOD zY0f-58Pb+zGOjIC^HjS)ghIufBs~jrFH%azx@BsavNE+oDY6S$==%vu$@sS{lRXg&dz$Mz+%FdU<$qCc4iD`_Iv97XEd|!)sRAaquME4=7!AoEv!3 z8r2jWheb`t$)tr8pD4)xo_YDa4AgV7e|$*f&x!mM5yD!Wv=ApT5Hq7Uf~FS)7%8L* zNdihmL`b*LPGZ7zMa59Hc{nZdk4!k6qW@5SQmkv$>YnnG^11TS@{#hw!c($eqsIRU zy=W7yK0Wwi4vh*dJTpQsa~&B@2sAsQ^Hf*NVlZLIe?-~mv+Okdi`TkS|3HQZCi)Mu q?4ioS|7C$$a`vh1$UV_^MDaq#&bM&`GH`7uRih3^uZq7#{eJ+4y2-}? literal 0 HcmV?d00001 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..bd4bef9f354ab13a6bfeb727b79715c2440c1371 GIT binary patch literal 165 zcmd1j<>g`k0^Z&WX(0MBh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o6v5KQS{oBQYl@ zH7`9?zdXMvySN}RIaNQYG&3hfKewPDGd?*#ucRm+sG2 SKczG$)edAsF%ytrVE_QkFDWem literal 0 HcmV?d00001 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3f99c96c85732caae4e643e101ce192d77ce0711 GIT binary patch literal 1831 zcmZux&1)n@6tAlO%p}>(W+lX3g<=mf4IzOQFGG-p90b95aPcrAO>I|Arf2%g>K@I8 z&P8+e>@{=rUl9B!Jl55N?4RJG;(OIIlMY%?ufBfo)vNb@ugcxs0fF|@N54(>144es zMZY<)cmcis0E8rxW@L;b%V^HVjFL}?bfkMjr27LKJCc4uBJT}g$$3PL_i|BXD#2yH zZNcIN^!iH>ij1iwV8e0_DpY|AChT1zeHlzb8Op62Z@eW3 z7UQFwxN=+W+_>X`48Vz9vwK$7&1a%|sfxPP5&Hu!vrW4&7&lo+VZsDJaV{#em8dL> z>-0()ztrhPS{PreW}%Do#9wLRfO8M}8T9%&h?ce_CM^R=Tj$zo*vx62)@?mWe{^8i zg*l16)&uEF2F^KRP6-+CRYM;*_`nMFZiuf46L2 zSEwNx!EuUi<2_v<@30XwerMKrR_8nlG-gr*>51`d(2U=yncdFmxLazh5WNi|@{L=o zY;vcmLp<96ekOVkHr{@IBGP0kvP=~h>g2mp&+AGg>g3fT&E!d5RcV}*MWZ3dO8;lQ z&gJU<@WzB@5}OIn=J@;{V=%9TJXk1gynE|bIzHs48Y8QO_Qh}cO= zEG`9%tef{?r``wgz(1sWbk8}U&)741;JEDS$$gUb)uL@=Xd!YH$7T@6c_|ke&UfPY zn}x`_6YIDw78BV2;lt;~@1k!!yP=V-8t>Qdp}=D53G`PxAdtv4B0|Ev5bD5v>oWEa zbJ*Vlm;J?p!JkhKEfRuZ!{x*80DowA0h;KUwYS(=d7hrv&dxpnf)EIhkTkMvg&U(liDK-8#Yk33e#ns^%aT1p#wB9DVmWsmifWu1cz8-g3#s2Q%N_v+t!%rYhwH zx~JdW)7}5~fB!#eqv2}!{rH9dXn*n-n)aWRnf#f^d<7wEBjOroiMCxn*|v_no){f- z+hkN`Bv!}XwpH3pDjjFrQE8S`x2uu1@9x*Qg=f93bDLN0o2P8M@pX+myn0{b)l+@D z$=L^5%YBHsw5=AC)`$IG5+Px}`>k)ijc4P{G|S%&dLKsJEEUogQQjBbkWq)hkJ1Lh zn+VxuM1_`X`|J|dPS0g+MdepD%%pEU)X}1)ODj)BxF_{qj9J*>r<$OB`Tcv>f;ij@ zk|gTxM%V78;^V9rgweH~ew^@Yon9~Y!?c@=6fMM)@|(RQxe#=BlgRI;q7x+Xaa5&M z(i$%7qK-$1F!gI7_Z5WfzYs;*HscyLo<+u)$@OzQ_pP?U4Q}3N+cvh>*spL~#g)jR zCxy1YrnPOg(dKrQKhJ0DbYA22`_{J08&Qo{c(ZNt1-^LS+^!GCyf7G(Mzi@6zlgfV zRNZpa#JE)N@Mri6>KCeO+#9sHtomPx7Wq2DaQ;QLGM7KgpTq1IcoT>6f?Sx{*sn=5 z?DHVBum!^(rD<=}G5-?|yb6ucv|DsASsr0f+UsUO5 zRC-yZS5*2LmA<6XD=NLpk@MF$jdNMWS2zjQ>XvBYaHJ9T`qJwAowy4jvw3t7hmo{+ ze$>MQ<-wt}vv{`?WU{iCiugF~%38PI@w1=<(a#qyHbBYL`V=05K?KpA9tnsK99SCkf*gSZOq$_ck!0dPJdTa zXb!a*^*Hl`gCI_V9iYVpHIYBq65B8L#LgA6%`8t4pow+r(HOl#9%HZUEKLp~A)coO z9z`!u^f`)Nr0DY$eSxBvC<4Z7aw+T!5p{FF7vy_>+~v`sY>KEIp)ib8sdUslouJpU zq?tuYTU@37EsALAVu>OG$ug=TB^@85#kud(`(|$;vYE|R^(L$9He1)%**en=hdE5g zpE>%Y66b%`7pT5&)Y!ibz72m4 z5(KvjF@A*v-D98z5Mv)0un){YY;s078&6=SdIAf80hR9&vRDYz*5Xzl+nU?lK4m{- zdF8BfuANYSLJ-C)57_tg6Z6Cx)Yyex7^~Vz1ud%Lo4L!KGeU+6uP$rcJp~H!+NsVo z;E>hUAwl)v-*BH?Z}9t0kKxZxe)Ke)Hy9o(53 zdHz(q*=foqUU_h8siVQSHoR9ShaZpqZKd^RUPDoZ+mmmx;Z5f5-1LHwWX8*)yM4Oo zUMXv%2?zMI;a$hHK^R5}s`4ljcj!sm|Ae027rj?Dyc?5?p4tO49V>U@r=| z&tnXWg~&^z(;A8-uJJ*Tv<&fOTvYMb6#WgNt>X_yO$WP`?qCo=lm}VeP)DBxU6u1X z{Vex(Xxa2=rvjG;Tn3WjDcJBjQNEY*%@z}Xg~5-TWoaIFqMP0iPU7D|d=&N`Ti)$X zcBj>l7coHSV-0>Tg8eAO4j+k^X?fQW0RRS@Sfa%%ROWsZB>m{CLV&QDG`6f@Dfa@w z6`Dw^C*p1{Z4`EbTv{UScX`VZuhOWrF7YKAhD2TD(k&0s$5F_tIu(^PqAr)NTE9j%x!Q3WbTA^?fu@ivb+3&n{-Np|r~b znu1=^6{N$}T6VCLv`d_QqH~=H;28)PH_vqd=&;v0LO{;)~MW`Dz|fXrWc^e z^K;;oj1jbBtaIa}GSzx&X4TppC;~ju+R3fLI%OwTL9^I}bq<^2q^fe2LC!5ICpA^# z6b{uB>|fDN>V;7#y0@%d(oPxxdm?_W$~9@+g2D23ZbI?sCl`tfxPMOYv&Se9k?y4V0L^xSEaDyjk;iSN`|~(RJTBsc z$Qx3o|GVN)GU@FEc?i{{$jk^}RiU53?3sji=(IeT!b#Ff1FW9a7X#(@sPouokybw`*%VBhnAMGAYp+u0K|&ZKs^IaG;%;vL7sZ7Ht!@DY9waY!Y|J|r~x zlt}~J6-a9y#L+#)9;Fq6o%SX;Az6yx)@de<4D{207AeV}qJb}zwRo?k2@w4|nuqw0 zWJfS}Y11V;kd{((gsxC@Qs;*VM+gUC>t!RW8{P8{rFtC6D&;0B&`v~IKgnm{L$TEW z(`Cd!SDUMh{xv7(#$Jk1+kLTSd`b{di?F#HjHA0f)WU=Is} zh}(&YC!`w0OFL7fuCTa4*Z?z+K|V}ij=~T~kd8W)Ue%W26F5`!?J?~YwhY>~Q;>0iS?VXKqnHXJ<84LPjbO1_(!~MnT3j*0JA&2gi9p$kzn=X!{GG z(dKZDaqX9>-YIIAw4#B~#CjKs1)x(kcm;dR{G!gGYB*Yd)?;&a}mX_y-R6smTd_WNy zhSKPz_r}(ttoFMZ7;$tQDNZ2kVcO~S^GGRwSsB)ij7We^zoRTkOfqI^3Sc6dq)pK- z&7?=LmZL;gm8D5%EB>C!odX|i4kSi44x%ItzAU7GYp@JqpZRxXH;%*)|#0SU7E72lCafQjh$Nmd=miFK-< z=s8?f(D7Aha7Yd}ARP?M1RSHSBL&7oM&<+K#3h!2Ru%9Wn47>a_)bfyg&eE&8qf#~ z%6R}Y^2~uDTFa|o8Zan{g`CuLx18aWy`|lG5t0nU6Ovia$pozz))noPVJ%It4M=EF z{a6z_Sf|Q?`?y8pmUj26n7=xabBmfXRNdg;K{H1;bS%jbtqCi1Y%X{p`jgh=raS8V zv||4MVVu(W&6}e`C&-*BI))5o>6W+GpD}ppHh5RTASMiG6h(*Lv;j?|i?)Z>FnW1n zJp=1$I0l5B$EDFasF`a0);nVY10|d$eP!Pc=9(LP-tZiEADW^tnLTRfDGPOKwvpjE zZKICI7%`{80!AIm_Z$uOSPXoe`Hlf$wU39HxI;6{a=34gGr&hjwE=GTlxHzzNs()d zq?rITTY=i5?+69Sv<7L!H;{oes&62ZC6G#JS}<}*JJ}-zy8<3^W}v?bZEF<9nRSjf z(26F?loJ{bXYd$cfTmp0RfPd?J8^O?Tim{d_Fx`v4$I&a_RT6{NcHP*Eg1!z%x87T zKX8WIaCm=o_ic>Z07@{FU>=NGg^d6w7p)yUsfwB^^ftb{kRzN%fVEN`=cyN!VT~@3 z09FtFoJ;6^OQRHU#J>A>VUfkLRq7z(W#S!*ND+Oim##wPG{ojVS}4h_N#8;M zq)`l~YMx39r~aiIB0&>rK_!9YTFTut9tJ)Fd`5|+fzLAFhI!PH4!j!b(~O8|?3y^j zLpe{#Eeu)!H%lg6@+|jK%#g`?p5{R^%sE7B6{M3>LF9f6Cd^{l%%a^6xk~8^P`+1c zwTtSe_fjG(d@#Zc{gn*}q;g@aQGbKgWX~bGuCKtu`5c02EW^)9o={KUV2jX7&__4t z^wGS#W%CvkW{_mBZB9*5?mca=_yM9VMGe#msIzD_#3|L$MHc@+(LYi|mU-!lwxN&V zrWXa3+(#r0c<03rRSGXG-arb6Hdgp*i;tDB2$4WO$`yz{^c?dC<1_V&r z5MwBP@e^ub!g0=L_Dp?bId;Qvb<=*9a zH!u!-X$N?SdlVf~!4mna@nsy2-Wdbfv7KI*~Gr&cGIXLpId zrmAlWa?Y`&2oM}Teu=Q<`}{4|B#r$TUb-w4aW5CYM2Y%fsTT0KHbb)V^-ot>Tiz~GA{0D^=_isH&Lpbxu}IEqM>6v?Sblo&FOv#~NBY!4PQ zoZVUV%#y+ys2s?YeL+=HIpoVu0j?xfy!h;#>{R{%R}MM!DXAP%ELAE_=*WtqnEAb) z+1&vMIh93C_j|8jf4zSFJ{obOVQcvP^-F)h_2Csw`)5LmKLf}$e94bcaEc#dV>S}bVce;IA@!HXu-cox>*WT24jo0sMyzaAm zrS>v+c!M|Z>+Kc)ET6f}#FoEuPeUL5rutg553wuldP|qa^_cspt#^3b^HZ_YDoNwr zIPqoq-8kr{(!MeDQeX6gxZh&Zz9qcS@1(KVkuy_Rv0{|duXb+xqUZJduf_dT#L;VU z4;V*Nn_1yKbqI8PxS}YKGQs+|*i*oO>JV%QuDPk<>bRVL1%2iH}y5d8800|ZxoC*O<=5uTU|E6)oKbj*2J3ye1@*@@Oj>tz-xuMo+_;80>4N+0qzO*S>o(aKQIopu>40l zHx$JvP)tQJbBYO`s-jd2l)9qS3zQi}naL>?^qN(a*_>j5GOsA}1)c>(Stw9WDaxq= z<&2`7DNvRaWvM_}R+QxeWkpd|3Y1kvSuIe`E6Vu-Wld4mrWA!gHO3WoVT|!h-)4WM z@9Cgc548iEKfSB}x%M4MSLstNt@F$LnZweq<~G1nOPl;kj+3+qZ{@fHd^Vlq&+cmB zJZLx-mt->wp3i}{nA1o)v*~I69B5~9+ETh4t{j{_)c!yh!*uyzm4E(F?;8qQKg8QI(f4%hUV?@LWEG#<vG|k zIdJ>9aI&%;i(nM@A1>jR$y$Hda}%#Oi2Nitm;t3cbbCR+i$?nWFb9701J&StFmAXu zZn!GVBkrqU;S!6<=V9t6Bn>;aS_GH zf+^h!x?frw&BeoXFibac+v;|D{2^Jc(R|)8h+waMu9i8nxxYE=ZqSlPPSB^0w}Zal z^U|P`jFvOXbZhCv9gG@TTL(tF=MALM8FEjSI)mY@mLbh-IkK#VmnQe`1ReMop6(3% z?)T6A{)acd{})$VS7bRE4tyak7Xk}X*Ojv{w2>!Bio%13LSv)rrv-HW2&=c~%2uPX#)nge1Mh5$r-`*5B(8VI3nC8!5-lQCwxZZeUwTnI z3ry6BWasJw7HP$KtRU^_ys|PLyq<3|4SI^X?`BY0RR|)C#E-VbGZ^-PE~~DK^Gy=h zeV~1EYr_jV+t|L}@A?~eV{tnfcpZOZa~MQ?byHtp zqm?J)j`hjiDD{$Vha!*hk6`*YaKDvCu^izeLXxC>hLfoKHe**<3e}mwgR2-Z2SBOU6EfgotIn}fO=bUfS zuqVxV6%E75oEJ~n=kY6aB;KP-JC=0vWgryqDG#l~Loa+PSh>KvPDriBN8#=taU*4m&tR9@(#X2bf1biNapw4Ch1)s)jC%#6 zw+^1&Rf$ddIVe5kd+;1)_&mP#Lv7a%H{9oWP1P?Tr6L(wNRmh5 zyO#H+`URvunS@uvm&U7BBh}PMU%*H;8cBtX&Fc^8Mj{MSZi_tG5nWOy4MegA2^qgG zWcS`BTv|QULOrBQN_8nU4$2BPbD9bLqBhd4thSVnXwVTotagn*WbZRvU0rw>wq;9W zKpC`!GEmn~|3K`D5DnITWVGs@BIAJkj{B z8lN(FH^XEfn~z5uC*0XM_u}g>V_q*xUI8#3w^f!1HHia~K!R6QhXDlVarC5-XJsX1t&Q+xE6DAcK4$F0{=-24p-J@lbz*!K1zenZ^-%$fP$KA1+f z=x(R7d^h0fwzSb0M?)2@t2pI#VyY+PmB)H-4hI;>mowz6(|9n>OvPp5EhAS&BubuA zw5j+VD!xX=dsNUHmiPq}Dy5(lL!}hrD@0c=`K#23b$J}g$8Qs2ZBatx23-6iq0MPl zM%kPAB@kQnEYx#}p^j()DNH6&o}A70I<##!@!yx#Yybjlg&nFe1Uok~ zR#`ui0%ZxMN`H>4;ICm-_wXfs6v`SQLZ*n^m_+57U8-uPEoLQ{}$yz^7 z*QQtF5~jwZ=ps%wPE(|^e3->{IGWBIqm^ld{6yc;nfy?nXt~uYXE*E%7+tA;7Npa~ zfSp!p!ZEw?4Kz`Jnk=9&^*J0?u|&wXeuDe>IhVfGq-?)Ws_aVJb$c;~O9WhZ$s~^TUfzlj7(Y*LN6{J*x7AYHfW~m&KTTs5Yh()D4O+e&a5-KdM&YJHv zOU;s1vT6<&)Oti0D1snYrF7v6fE4kcd{jCg=I9xK@%raZojhj#I8j4lm&^(ri!)+~*l7xW(iO2`{P&2<~f zF1|}`5G1v_ZkBSbitiBBR`ac^!DtJz-uLcM0+Fnv$lf1jyMFfmKc$n_D?g(gj66^8 zUw7#xMm>b7OHO&jWSk1|)0*VDqpahXrwM13IO*JEHCx*IaUyDjV{=e&@&MvngtPu2 z^K9pPNB=*tehklEDW2dOo9EoTWoDnwRmn^I2|-f@SCA62^>U_$uh8{R=%}F`5c7hv VECh`sCxRWO{hrM&)7Fj0{|l9Y2e1GD literal 0 HcmV?d00001 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py new file mode 100644 index 00000000..fd977e60 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py @@ -0,0 +1,49 @@ +import torch +from typing import Tuple + +class CostMapTensor: + def __init__( + self, + cost_map: torch.Tensor, + cell_size: float, + origin: Tuple[float, float] = (0.0, 0.0), + device=torch.device("cuda"), + dtype=torch.float32, + ) -> None: + """ + cost map tensor for collision checking. + input: + cost_map (torch.Tensor): cost map tensor. + cell_size (float): size(m) of each cell in the cost map. + origin (Tuple[float, float]): origin of the cost map. (m, m) + device: device to run the computation. + dtype: data type of the tensor. + """ + self.cost_map: torch.Tensor = cost_map + self.cell_size: float = cell_size + origin: Tuple[float, float] = origin + self.origin_tensor = torch.tensor(origin, device=device, dtype=dtype) + self.device = device + self.dtype = dtype + + def compute_cost(self, x: torch.Tensor) -> torch.Tensor: + """ + Check collision in a batch of trajectories. + :param x: Tensor of shape (batch_size, traj_length, position_dim). + :return: collsion costs on the trajectories. + """ + assert self.cost_map is not None + if x.device != self.device or x.dtype != self.dtype: + x = x.to(self.device, self.dtype) + + # project to cell map + x_occ = (x - self.origin_tensor) / self.cell_size + x_occ = torch.round(x_occ).long().to(self.device) + + x_occ[..., 0] = torch.clamp(x_occ[..., 0], 0, self.cost_map.shape[0] - 1) + x_occ[..., 1] = torch.clamp(x_occ[..., 1], 0, self.cost_map.shape[1] - 1) + + # collision check + collisions = self.cost_map[x_occ[..., 1], x_occ[..., 0]] + + return collisions \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py new file mode 100644 index 00000000..30995175 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py @@ -0,0 +1,244 @@ +import torch +import numpy as np +from typing import Tuple +from mppi_controller.MPPI import MPPI +from mppi_controller.cost_map_tensor import CostMapTensor + +import time + +@torch.jit.script +def angle_normalize(x) -> torch.Tensor: + return ((x + torch.pi) % (2 * torch.pi)) - torch.pi + +class mppi_controller: + def __init__(self, config, debug=False, device=torch.device("cuda"), dtype=torch.float32): + self.config = config + self.debug = debug + # device and dtype + if torch.cuda.is_available() and device == torch.device("cuda"): + self._device = torch.device("cuda") + else: + self._device = torch.device("cpu") + self._dtype = dtype + + self.u_min = torch.tensor(self.config["u_min"], device=self._device, dtype=self._dtype) + self.u_max = torch.tensor(self.config["u_max"], device=self._device, dtype=self._dtype) + self.sigmas = torch.tensor(self.config["sigmas"], device=self._device, dtype=self._dtype) + + # solver + self.solver = MPPI( + horizon=self.config["horizon"], + num_samples=self.config["num_samples"], + dim_state=4, + dim_control=2, + dynamics=self.dynamics, + cost_func=self.cost_function, + u_min=self.u_min, + u_max=self.u_max, + sigmas=self.sigmas, + lambda_=self.config["lambda"], + auto_lambda=self.config["auto_lambda"], + ) + + # model parameter + self.delta_t = torch.tensor(self.config["delta_t"], device=self._device, dtype=self._dtype) + self.vehicle_L = torch.tensor(self.config["vehicle_L"], device=self._device, dtype=self._dtype) + self.V_MAX = torch.tensor(self.config["V_MAX"], device=self._device, dtype=self._dtype) + + # cost weights + self.Qc = self.config["Qc"] # contouring error cost + self.Ql = self.config["Ql"] # lag error cost + self.Qv = self.config["Qv"] # velocity cost + self.Qo = self.config["Qo"] # obstacle cost + self.Qin = self.config["Qin"] # input cost + self.Qdin = self.config["Qdin"] # input rate cost + + self.current_path_index = 0 + + # reference indformation (tensor) + self.reference_path: torch.Tensor = None + self.cost_map: CostMapTensor = None + + def update(self, state: torch.Tensor, racing_center_path: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """ + Update the controller with the current state and reference path. + Args: + state (torch.Tensor): current state of the vehicle, shape (4,) [x, y, yaw, v] + racing_center_path (torch.Tensor): racing center path, shape (N, 3) [x, y, yaw] + Returns: + Tuple[torch.Tensor, torch.Tensor]: action sequence tensor, shape (horizon, 2) [accel, steer], state sequence tensor, shape (horizon + 1, 4) [x, y, yaw, v] + """ + + # reference + self.reference_path, self.current_path_index = self.calc_ref_trajectory( + state, racing_center_path, self.current_path_index, self.solver._horizon, DL=self.config["DL"], lookahead_distance=self.config["lookahead_distance"], reference_path_interval=self.config["reference_path_interval"] + ) + + if self.reference_path is None: + raise ValueError("reference path, obstacle map, and lane map must be set before calling solve method.") + + # solve + start = time.time() + action_seq, state_seq = self.solver.forward(state=state) + end = time.time() + solve_time = end - start + + if self.debug: + print("solve time: {}".format(round(solve_time * 1000, 2)), " [ms]") + + return action_seq, state_seq + + def get_top_samples(self, num_samples = 300) -> Tuple[torch.Tensor, torch.Tensor]: + return self.solver.get_top_samples(num_samples=num_samples) + + def set_cost_map(self, cost_map: CostMapTensor) -> None: + self.cost_map = cost_map + + def dynamics( + self, state: torch.Tensor, action: torch.Tensor + ) -> torch.Tensor: + """ + Update robot state based on differential drive dynamics. + Args: + state (torch.Tensor): state batch tensor, shape (batch_size, 3) [x, y, theta, v] + action (torch.Tensor): control batch tensor, shape (batch_size, 2) [accel, steer] + delta_t (float): time step interval [s] + Returns: + torch.Tensor: shape (batch_size, 3) [x, y, theta] + """ + + # Perform calculations as before + x = state[:, 0].view(-1, 1) + y = state[:, 1].view(-1, 1) + theta = state[:, 2].view(-1, 1) + v = state[:, 3].view(-1, 1) + accel = torch.clamp(action[:, 0].view(-1, 1), self.u_min[0], self.u_max[0]) + steer = torch.clamp(action[:, 1].view(-1, 1), self.u_min[1], self.u_max[1]) + theta = angle_normalize(theta) + + dx = v * torch.cos(theta) + dy = v * torch.sin(theta) + dv = accel + dtheta = v * torch.tan(steer) / self.vehicle_L + + new_x = x + dx * self.delta_t + new_y = y + dy * self.delta_t + new_theta = angle_normalize(theta + dtheta * self.delta_t) + new_v = v + dv * self.delta_t + + # Clamp velocity + new_v = torch.clamp(new_v, -self.V_MAX, self.V_MAX) + + result = torch.cat([new_x, new_y, new_theta, new_v], dim=1) + + return result + + + def cost_function(self, state: torch.Tensor, action: torch.Tensor, info: dict) -> torch.Tensor: + """ + Calculate cost function + Args: + state (torch.Tensor): state batch tensor, shape (batch_size, 4) [x, y, theta, v] + action (torch.Tensor): control batch tensor, shape (batch_size, 2) [accel, steer] + Returns: + torch.Tensor: shape (batch_size,) + """ + # info + prev_action = info["prev_action"] + t = info["t"] # horizon number + + # path cost + # contouring and lag error of path + ec = torch.sin(self.reference_path[t, 2]) * (state[:, 0] - self.reference_path[t, 0]) \ + -torch.cos(self.reference_path[t, 2]) * (state[:, 1] - self.reference_path[t, 1]) + el = -torch.cos(self.reference_path[t, 2]) * (state[:, 0] - self.reference_path[t, 0]) \ + -torch.sin(self.reference_path[t, 2]) * (state[:, 1] - self.reference_path[t, 1]) + + path_cost = self.Qc * ec.pow(2) + self.Ql * el.pow(2) + + # velocity cost + v = state[:, 3] + v_target = self.reference_path[t, 3] + velocity_cost = self.Qv * (v - v_target).pow(2) + + # compute obstacle cost from cost map + pos_batch = state[:, :2].unsqueeze(1) # (batch_size, 1, 2) + obstacle_cost = self.cost_map.compute_cost(pos_batch).squeeze(1) # (batch_size,) + obstacle_cost = self.Qo * obstacle_cost + + # input cost + input_cost = self.Qin * action.pow(2).sum(dim=1) + input_cost += self.Qdin * (action - prev_action).pow(2).sum(dim=1) + + cost = path_cost + velocity_cost + obstacle_cost + input_cost + + return cost + + def calc_ref_trajectory(self, state: torch.Tensor, path: torch.Tensor, + cind: int, horizon: int, DL=0.1, lookahead_distance=1.0, reference_path_interval=0.5 + ) -> Tuple[torch.Tensor, int]: + """ + Calculate the reference trajectory for the vehicle. + + Args: + state (torch.Tensor): current state of the vehicle, shape (4,) [x, y, yaw, v] + path (torch.Tensor): reference path, shape (N, 4) [x, y, yaw, target_v] + cind (int): current index of the vehicle on the path + horizon (int): prediction horizon + DL (float): resolution of the path + lookahead_distance (float): distance to look ahead + reference_path_interval (float): interval of the reference path + + Returns: + Tuple[torch.Tensor, int]: reference trajectory tensor, shape (horizon + 1, 4) [x, y, yaw, target_v], index of the vehicle on the path + """ + + def resample_path(path, DL): + # Calculate the number of segments needed for each pair of points + distances = torch.norm(path[1:, :2] - path[:-1, :2], dim=1) + num_points = torch.ceil(distances / DL).to(torch.int64) + + # Create a tensor to store the new resampled path points + total_points = num_points.sum() + 1 # Include the first point + new_path = torch.zeros((total_points, path.shape[1]), dtype=path.dtype, device=path.device) + + # Initialize the first point + new_path[0] = path[0] + + # Generate all new points at once + start_idx = 1 + for i in range(len(path) - 1): + segment_length = num_points[i].item() + if segment_length == 0: + continue + + t = torch.linspace(0, 1, segment_length + 1, device=path.device)[1:] # Skip the first point to avoid duplication + interpolated_points = (1 - t).unsqueeze(1) * path[i] + t.unsqueeze(1) * path[i + 1] + new_path[start_idx:start_idx + segment_length] = interpolated_points + start_idx += segment_length + + return new_path + + # Resample the path with the specified DL + path = resample_path(path, DL) + ncourse = len(path) + xref = torch.zeros((horizon + 1, state.shape[0]), dtype=state.dtype, device=state.device) + + # Calculate the nearest index to the vehicle + ind = torch.argmin(torch.norm(path[:, :2] - state[:2], dim=1)).item() + # Ensure the index is not less than the current index + ind = max(cind, ind) + + # Generate the rest of the reference trajectory + travel = lookahead_distance + + for i in range(horizon + 1): + travel += reference_path_interval + dind = int(round(travel / DL)) + + if (ind + dind) < ncourse: + xref[i] = path[ind + dind] + else: + xref[i] = path[-1] + + return xref, ind \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py new file mode 100644 index 00000000..a0153179 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py @@ -0,0 +1,250 @@ +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose, Point, Quaternion +from autoware_auto_planning_msgs.msg import Trajectory, TrajectoryPoint +from autoware_auto_control_msgs.msg import AckermannControlCommand +from nav_msgs.msg import OccupancyGrid +from rclpy.qos import QoSProfile +import math +import torch +import numpy as np +import tf_transformations +from mppi_controller.mppi_controller import mppi_controller +from mppi_controller.cost_map_tensor import CostMapTensor + +class MppiControllerNode(Node): + + def __init__(self): + super().__init__('mppi_controller_node') + + # parameters + # declare + self.declare_parameter('horizon', 25) + self.declare_parameter('num_samples', 4000) + self.declare_parameter('u_min', [-2.0, -0.25]) + self.declare_parameter('u_max', [2.0, 0.25]) + self.declare_parameter('sigmas', [0.5, 0.1]) + self.declare_parameter('lambda', 1.0) + self.declare_parameter('auto_lambda', False) + self.declare_parameter('DL', 0.1) + self.declare_parameter('lookahead_distance', 3.0) + self.declare_parameter('reference_path_interval', 0.85) + self.declare_parameter('Qc', 2.0) + self.declare_parameter('Ql', 3.0) + self.declare_parameter('Qv', 2.0) + self.declare_parameter('Qo', 10000.0) + self.declare_parameter('Qin', 0.01) + self.declare_parameter('Qdin', 0.5) + self.declare_parameter('delta_t', 0.1) + self.declare_parameter('vehicle_L', 1.0) + self.declare_parameter('V_MAX', 8.0) + # get + config = { + "horizon": self.get_parameter('horizon').get_parameter_value().integer_value, + "num_samples": self.get_parameter('num_samples').get_parameter_value().integer_value, + "u_min": self.get_parameter('u_min').get_parameter_value().double_array_value, + "u_max": self.get_parameter('u_max').get_parameter_value().double_array_value, + "sigmas": self.get_parameter('sigmas').get_parameter_value().double_array_value, + "lambda": self.get_parameter('lambda').get_parameter_value().double_value, + "auto_lambda": self.get_parameter('auto_lambda').get_parameter_value().bool_value, + "DL": self.get_parameter('DL').get_parameter_value().double_value, + "lookahead_distance": self.get_parameter('lookahead_distance').get_parameter_value().double_value, + "reference_path_interval": self.get_parameter('reference_path_interval').get_parameter_value().double_value, + "Qc": self.get_parameter('Qc').get_parameter_value().double_value, + "Ql": self.get_parameter('Ql').get_parameter_value().double_value, + "Qv": self.get_parameter('Qv').get_parameter_value().double_value, + "Qo": self.get_parameter('Qo').get_parameter_value().double_value, + "Qin": self.get_parameter('Qin').get_parameter_value().double_value, + "Qdin": self.get_parameter('Qdin').get_parameter_value().double_value, + "delta_t": self.get_parameter('delta_t').get_parameter_value().double_value, + "vehicle_L": self.get_parameter('vehicle_L').get_parameter_value().double_value, + "V_MAX": self.get_parameter('V_MAX').get_parameter_value().double_value, + } + + self.get_logger().info(f'config: {config}') + + # publisher + # control command + self.pub_cmd = self.create_publisher(AckermannControlCommand, 'output/control_cmd', 1) + # planned path + self.pub_planned_path = self.create_publisher(Trajectory, 'output/planned_path', 1) + # debug path + self.pub_debug_path = self.create_publisher(Trajectory, 'debug/path', 1) + + # subscriber + # state + self.sub_kinematics = self.create_subscription( + Odometry, + 'input/kinematics', + self.kinematics_callback, + 1 + ) + # reference trajectory + self.sub_trajectory = self.create_subscription( + Trajectory, + 'input/reference_trajectory', + self.trajectory_callback, + 1 + ) + # costmap + self.sub_costmap = self.create_subscription( + OccupancyGrid, + 'input/costmap', + self.costmap_callback, + 1 + ) + + # device and dtype + self.device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") + self.dtype = torch.float32 + + # mppi controller + self.controller = mppi_controller(config=config, debug=True, device=self.device, dtype=self.dtype) + + self.odometry: Odometry = None + self.trajectory: Trajectory = None + self.costmap: OccupancyGrid = None + + self.timer = self.create_timer(0.03, self.on_timer) + + def kinematics_callback(self, msg : Odometry): + self.odometry = msg + + def trajectory_callback(self, msg : Trajectory): + self.trajectory = msg + + def costmap_callback(self, msg : OccupancyGrid): + self.costmap = msg + + def zero_ackermann_control_command(self): + cmd = AckermannControlCommand() + now = self.get_clock().now().to_msg() + cmd.stamp = now + cmd.longitudinal.stamp = now + cmd.longitudinal.speed = 0.0 + cmd.longitudinal.acceleration = 0.0 + cmd.lateral.stamp = now + cmd.lateral.steering_tire_angle = 0.0 + return cmd + + def on_timer(self): + if not self.subscribe_message_available(): + return + + cmd = self.zero_ackermann_control_command() + + # convert tensor + # state + state_tensor = torch.tensor([ + self.odometry.pose.pose.position.x, + self.odometry.pose.pose.position.y, + tf_transformations.euler_from_quaternion([ + self.odometry.pose.pose.orientation.x, + self.odometry.pose.pose.orientation.y, + self.odometry.pose.pose.orientation.z, + self.odometry.pose.pose.orientation.w + ])[2], + self.odometry.twist.twist.linear.x + ], dtype=self.dtype, device=self.device) + # reference path + reference_path_tensor = torch.tensor([ + [point.pose.position.x, + point.pose.position.y, + tf_transformations.euler_from_quaternion([ + point.pose.orientation.x, + point.pose.orientation.y, + point.pose.orientation.z, + point.pose.orientation.w + ])[2], + point.longitudinal_velocity_mps] for point in self.trajectory.points + ], dtype=self.dtype, device=self.device) + # convert OccupancyGrid to tensor + costmap_tensor: CostMapTensor = CostMapTensor( + cost_map=torch.tensor(self.costmap.data, dtype=self.dtype, device=self.device).reshape(self.costmap.info.height, self.costmap.info.width), + cell_size=self.costmap.info.resolution, + origin=(self.costmap.info.origin.position.x, + self.costmap.info.origin.position.y), + device=self.device, + dtype=self.dtype + ) + # set cost map + self.controller.set_cost_map(costmap_tensor) + + # update controller + action_seq, state_seq = self.controller.update(state_tensor, reference_path_tensor) + # get top samples + top_samples = self.controller.get_top_samples(num_samples=300) + + # convert numpy + state = state_seq.cpu().numpy()[0] + action = action_seq.cpu().numpy()[0] + + # publish control command + cmd.longitudinal.speed = float(state[0, 3]) + cmd.longitudinal.acceleration = float(action[0]) + cmd.lateral.steering_tire_angle = float(action[1]) + self.pub_cmd.publish(cmd) + + # publish planned path + planned_path = Trajectory() + planned_path.header.stamp = self.get_clock().now().to_msg() + planned_path.header.frame_id = 'map' + planned_path.points = [ + TrajectoryPoint( + pose=Pose( + position=Point(x=float(point[0]), y=float(point[1])), + orientation=Quaternion( + x=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[0], + y=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[1], + z=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[2], + w=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[3] + ) + ), + longitudinal_velocity_mps=float(point[3]) + ) + for point in state] + + self.pub_planned_path.publish(planned_path) + + # publish debug path + debug_path = Trajectory() + debug_path.header.stamp = self.get_clock().now().to_msg() + debug_path.header.frame_id = 'map' + debug_path.points = [ + TrajectoryPoint( + pose=Pose( + position=Point(x=float(point[0]), y=float(point[1])), + orientation=Quaternion( + x=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[0], + y=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[1], + z=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[2], + w=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[3] + ) + ), + longitudinal_velocity_mps=float(point[3]) + ) + for point in self.controller.reference_path.cpu().numpy()] + self.pub_debug_path.publish(debug_path) + + + def subscribe_message_available(self): + if not self.odometry: + self.get_logger().info('odometry is not available', throttle_duration_sec=1) + return False + if not self.trajectory: + self.get_logger().info('trajectory is not available', throttle_duration_sec=1) + return False + if not self.costmap: + self.get_logger().info('costmap is not available', throttle_duration_sec=1) + return False + return True + +def main(args=None): + rclpy.init(args=args) + node = MppiControllerNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml new file mode 100644 index 00000000..e35b459d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml @@ -0,0 +1,16 @@ + + + + mppi_controller + 0.0.0 + TODO: Package description + eguchi + TODO: License declaration + + rclpy + rclpy + + + ament_python + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller new file mode 100644 index 00000000..e69de29b diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg new file mode 100644 index 00000000..7d2409e1 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mppi_controller +[install] +install_scripts=$base/lib/mppi_controller \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py new file mode 100644 index 00000000..37ccad01 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py @@ -0,0 +1,33 @@ +import subprocess +import sys + +# dependency +subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'setuptools==58.0.4', '--quiet']) +subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'torch', '--index-url', 'https://download.pytorch.org/whl/cpu', '--quiet']) +subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'transforms3d', '--quiet']) + +from setuptools import setup, find_packages +package_name = 'mppi_controller' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', ['config/mppi_controller.param.yaml']), + ], + install_requires=['setuptools', 'scipy'], + zip_safe=True, + maintainer='michikuni eguchi', + maintainer_email='egrt117@gmail.com', + description='MPPI controller', + license='License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'mppi_controller_node = mppi_controller.mppi_controller_node:main', + ], + }, +) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index 2d248d76..9e75c143 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -5,9 +5,13 @@ Panels: Property Tree Widget: Expanded: - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /Planning1 + - /Planning1/ScenarioPlanning1/ScenarioTrajectory1 + - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1 + - /Planning1/Costmap1/Map1 + - /Planning1/MPPI1 + - /Planning1/MPPI1/Trajectory1 Splitter Ratio: 0.557669460773468 - Tree Height: 242 + Tree Height: 158 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -865,7 +869,7 @@ Visualization Manager: - Class: rviz_common/Group Displays: - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 + Color Border Vel Max: 8 Enabled: true Name: ScenarioTrajectory Topic: @@ -889,7 +893,7 @@ Visualization Manager: Color: 0; 0; 0 Constant Color: false Value: true - Width: 2 + Width: 0.5 View Point: Alpha: 1 Color: 0; 60; 255 @@ -1941,7 +1945,7 @@ Visualization Manager: Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map - Color Scheme: map + Color Scheme: costmap Draw Behind: false Enabled: true Name: Map @@ -1962,6 +1966,51 @@ Visualization Manager: Value: true Enabled: true Name: Costmap + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 8 + Enabled: true + Name: Trajectory + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 11 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/output/mppi_planned_path + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 0.5 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + Enabled: true + Name: MPPI Enabled: true Name: Planning - Class: rviz_common/Group @@ -2170,11 +2219,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 10.328941345214844 + Scale: 7.122405529022217 Target Frame: viewer Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 + X: 2.5461606979370117 + Y: -8.487200736999512 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 18 @@ -2220,12 +2269,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1376 + Height: 1043 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -2234,6 +2283,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3370 - X: 70 - Y: 27 + Width: 1920 + X: 0 + Y: 32 diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml index eda86a3a..9969163f 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml @@ -9,7 +9,7 @@ - + From 1b596fb24bd9e6a5974a0a424fd40fedb4ae3a5f Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Sat, 31 Aug 2024 15:43:41 +0900 Subject: [PATCH 17/48] chore: costmap integration (#22) * launch costmap_generator Signed-off-by: Autumn60 * add costmap to rviz Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .../planning/costmap_generator.param.yaml | 18 + .../launch/components/planning.launch.xml | 11 + .../launch/costmap_generator.launch.xml | 3 + .../config/autoware.rviz | 486 ++++++++++++++---- 4 files changed, 431 insertions(+), 87 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml new file mode 100644 index 00000000..761d16e7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + update_rate: 10.0 + map_frame_id: "map" + costmap_center_frame_id: "base_link" + costmap_width: 50.0 + costmap_resolution: 0.2 + multi_layered_costmap: + layers: + - "lanelet_layer" + - "predicted_object_layer" + lanelet_layer: + type: "lanelet" + map_topic: "/map/vector_map" + predicted_object_layer: + type: "predicted_object" + predicted_objects_topic: "/perception/object_recognition/objects" + distance_threshold: 1.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index 2c8c989f..483d4cd0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -100,5 +100,16 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml index 9501d94e..0dea8897 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml @@ -1,7 +1,10 @@ + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index 1539328f..2d248d76 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -3,9 +3,11 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Planning1 Splitter Ratio: 0.557669460773468 - Tree Height: 397 + Tree Height: 242 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -13,7 +15,8 @@ Panels: Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views - Expanded: ~ + Expanded: + - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel @@ -39,7 +42,8 @@ Visualization Manager: Show Arrows: true Show Axes: true Show Names: true - Tree: {} + Tree: + {} Update Interval: 0 Value: false - Alpha: 0.5 @@ -64,9 +68,12 @@ Visualization Manager: Displays: - Class: rviz_plugins/SteeringAngle Enabled: true + Left: 85 + Length: 171 Name: SteeringAngle Scale: 17 Text Color: 25; 255; 240 + Top: 85 Topic: Depth: 5 Durability Policy: Volatile @@ -74,12 +81,15 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/steering_status Value: true + Value Scale: 0.14999249577522278 Value height offset: 0 - Class: rviz_plugins/ConsoleMeter Enabled: true + Left: 341 + Length: 171 Name: ConsoleMeter - Scale: 3 Text Color: 25; 255; 240 + Top: 85 Topic: Depth: 5 Durability Policy: Volatile @@ -87,8 +97,9 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/velocity_status Value: true + Value Scale: 0.14999249577522278 Value height offset: 0 - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Class: rviz_plugins/VelocityHistory Color Border Vel Max: 3 Constant Color: @@ -157,8 +168,8 @@ Visualization Manager: Max Alpha: 0.5 Max Range: 100 Max Wave Alpha: 0.5 - Min Alpha: 0.01 - Min Wave Alpha: 0.01 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 Name: PolarGridDisplay Reference Frame: base_link Value: true @@ -166,18 +177,20 @@ Visualization Manager: Wave Velocity: 40 - Class: rviz_plugins/MaxVelocity Enabled: true + Left: 397 + Length: 64 Name: MaxVelocity Text Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/current_max_velocity + Top: 187 + Topic: /planning/scenario_planning/current_max_velocity Value: true + Value Scale: 0.25 - Class: rviz_plugins/TurnSignal Enabled: true + Height: 171 + Left: 131 Name: TurnSignal + Top: 233 Topic: Depth: 5 Durability Policy: Volatile @@ -185,6 +198,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /vehicle/status/turn_indicators_status Value: true + Width: 341 Enabled: true Name: Vehicle Enabled: true @@ -213,11 +227,12 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 1 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Transient Local + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /map/pointcloud_map @@ -248,7 +263,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.4 + - Alpha: 0.4000000059604645 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 5 @@ -270,18 +285,19 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 1 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /sensing/lidar/concatenated/pointcloud Use Fixed Frame: false Use rainbow: true Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Class: rviz_default_plugins/Polygon Color: 25; 255; 0 Enabled: false @@ -289,6 +305,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /sensing/lidar/crop_box_filter/crop_box_polygon @@ -327,6 +344,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /sensing/gnss/pose_with_covariance @@ -339,7 +357,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/PoseWithCovariance @@ -369,11 +387,12 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/initial_pose_with_covariance - Value: true - - Alpha: 0.999 + Value: false + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/PoseWithCovariance @@ -403,27 +422,29 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/pose_with_covariance - Value: true + Value: false - Buffer Size: 200 Class: rviz_plugins::PoseHistory Enabled: false Line: + Alpha: 0.9990000128746033 Color: 170; 255; 127 Value: true Width: 0.10000000149011612 - Alpha: 0.999 Name: PoseHistory Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/pose - Value: true - - Alpha: 0.999 + Value: false + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -450,13 +471,14 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /localization/util/downsample/pointcloud Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -483,6 +505,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_estimator/points_aligned @@ -492,7 +515,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MonteCarloInitialPose - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -508,14 +532,15 @@ Visualization Manager: Class: rviz_plugins::PoseHistory Enabled: true Line: + Alpha: 0.9990000128746033 Color: 0; 255; 255 Value: true Width: 0.10000000149011612 - Alpha: 0.999 Name: PoseHistory Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /localization/pose_twist_fusion_filter/pose @@ -528,7 +553,7 @@ Visualization Manager: Displays: - Class: rviz_common/Group Displays: - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 15 @@ -550,11 +575,12 @@ Visualization Manager: Position Transformer: XYZ Selectable: false Size (Pixels): 3 - Size (m): 0.02 + Size (m): 0.019999999552965164 Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /perception/obstacle_segmentation/pointcloud @@ -568,16 +594,16 @@ Visualization Manager: - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: true Display Predicted Path Confidence: true @@ -585,21 +611,23 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: DetectedObjects - Namespaces: {} + Namespaces: + {} PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -608,24 +636,25 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/detection/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal Enabled: false Name: Detection - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: true Display Predicted Path Confidence: true @@ -633,21 +662,23 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: TrackedObjects - Namespaces: {} + Namespaces: + {} PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -656,24 +687,25 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/tracking/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal Enabled: false Name: Tracking - Class: rviz_common/Group Displays: - BUS: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CAR: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 CYCLIST: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display 3d polygon: true + Display Acceleration: true Display Label: true Display PoseWithCovariance: false Display Predicted Path Confidence: true @@ -681,21 +713,28 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true - Line Width: 0.03 Enabled: true + Line Width: 0.029999999329447746 MOTORCYCLE: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: PredictedObjects - Namespaces: {} + Namespaces: + acceleration: true + label: true + shape: true + twist: true + uuid: true + velocity: true PEDESTRIAN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 192; 203 + Polygon Type: 3d TRAILER: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 TRUCK: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 30; 144; 255 Topic: Depth: 5 @@ -704,20 +743,22 @@ Visualization Manager: Reliability Policy: Best Effort Value: /perception/object_recognition/objects UNKNOWN: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 255; 255; 255 Value: true + Visualization Type: Normal - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Maneuver - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort Value: /perception/object_recognition/prediction/maneuver - Value: true + Value: false Enabled: true Name: Prediction Enabled: true @@ -741,7 +782,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MapBasedDetectionResult - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -798,7 +840,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/mission_planning/route_marker Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Axes Length: 1 Axes Radius: 0.30000001192092896 Class: rviz_default_plugins/Pose @@ -813,6 +855,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/mission_planning/echo_back_goal_pose @@ -828,18 +871,36 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/trajectory Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Scale: 0.30000001192092896 @@ -855,16 +916,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/path Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.4000000059604645 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.4000000059604645 Color: 0; 0; 0 @@ -878,16 +962,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/lane_change Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -901,16 +1008,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/ext_request_lane_change_right Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -924,16 +1054,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/ext_request_lane_change_left Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -947,16 +1100,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/avoidance Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -970,16 +1146,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/pull_out Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -993,16 +1192,39 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/path_candidate/pull_over Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 0.30000001192092896 Color: 0; 0; 0 @@ -1018,7 +1240,6 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound @@ -1414,18 +1635,36 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/trajectory Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Value: true Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: - Alpha: 0.999 + Alpha: 0.9990000128746033 Color: 0; 0; 0 Constant Color: false Scale: 0.30000001192092896 @@ -1608,6 +1847,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid @@ -1619,7 +1859,7 @@ Visualization Manager: Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates Use Timestamp: false Value: false - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Arrow Length: 0.30000001192092896 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 @@ -1635,11 +1875,12 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array Value: true - - Alpha: 0.999 + - Alpha: 0.9990000128746033 Arrow Length: 0.5 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 @@ -1655,6 +1896,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array @@ -1684,7 +1926,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: true Name: PlanningErrorMarker - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1694,6 +1937,31 @@ Visualization Manager: Value: true Enabled: false Name: Diagnostic + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/costmap/costmap_generator/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/costmap/costmap_generator/costmap_updates + Use Timestamp: false + Value: true + Enabled: true + Name: Costmap Enabled: true Name: Planning - Class: rviz_common/Group @@ -1705,16 +1973,34 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /control/trajectory_follower/lateral/predicted_trajectory Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 1 Color: 255; 255; 255 Constant Color: true Value: true Width: 0.05000000074505806 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false View Velocity: Alpha: 1 Color: 0; 0; 0 @@ -1724,7 +2010,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Debug/MPC - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1735,7 +2022,8 @@ Visualization Manager: - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Debug/PurePursuit - Namespaces: {} + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1797,15 +2085,15 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose - Theta std deviation: 0.2617993950843811 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 @@ -1813,26 +2101,47 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/mission_planning/goal - - Class: rviz_plugins/PedestrianInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 0 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 1 Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/CarInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 3 + W vehicle width: 1.7999999523162842 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 0 Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/BusInitialPoseTool + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: Theta std deviation: 0.0872664600610733 Velocity: 0 + W vehicle width: 2.5 X std deviation: 0.029999999329447746 Y std deviation: 0.029999999329447746 Z position: 0 @@ -1845,6 +2154,9 @@ Visualization Manager: Z position: 0 - Class: rviz_plugins/DeleteAllObjectsTool Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: @@ -1858,7 +2170,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 10 + Scale: 10.328941345214844 Target Frame: viewer Value: TopDownOrtho (rviz_default_plugins) X: 0 @@ -1908,12 +2220,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1016 + Height: 1376 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 000000ff00000000fd00000004000000000000029700000531fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000140000012d000000c700fffffffc000001470000010b000000bb0100001afa000000000100000002fb0000000a0056006900650077007301000000000000033c0000010000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000008c00fffffffb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c0100000258000001fd0000015e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007100fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065000000038a0000010b0000002800fffffffb0000002a004100750074006f0077006100720065004400610074006500540069006d006500500061006e0065006c010000045b0000005e0000003f00fffffffb00000034004100750074006f007700610072006500530063007200650065006e004300610070007400750072006500500061006e0065006c01000004bf000000860000005d00ffffff000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a0000005afc0100000002fb0000000800540069006d0065010000000000000e7a0000000000000000fb0000000800540069006d00650100000000000004500000000000000000000009df0000053100000004000000040000000800000008fc0000000100000000000000010000000a0054006f006f006c00730300000000ffffffff0000000000000000 RecognitionResultOnImage: collapsed: false Selection: @@ -1922,6 +2234,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 + Width: 3370 X: 70 Y: 27 From a16db558f8fcd6bf9d5e97bed8d7155dcc76de93 Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:08:24 +0900 Subject: [PATCH 18/48] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../mppi_controller/config/mppi_controller.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml index 96d2990a..15fa92cd 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -10,7 +10,7 @@ auto_lambda : false # reference path DL : 0.1 - loolahead_distance : 3.0 + lookahead_distance : 3.0 reference_path_interval : 0.85 # cost weights Qc : 2.0 From 257cd04032778c7ea4283f9fe89932dc6187ca9f Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:08:48 +0900 Subject: [PATCH 19/48] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../mppi_controller/mppi_controller/mppi_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py index 30995175..7a65192f 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py @@ -55,7 +55,7 @@ def __init__(self, config, debug=False, device=torch.device("cuda"), dtype=torch self.current_path_index = 0 - # reference indformation (tensor) + # reference information (tensor) self.reference_path: torch.Tensor = None self.cost_map: CostMapTensor = None From 089dc4ddf81476b3a7a7002769643962828dc74e Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:09:06 +0900 Subject: [PATCH 20/48] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../mppi_controller/mppi_controller/cost_map_tensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py index fd977e60..747bc749 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py @@ -30,7 +30,7 @@ def compute_cost(self, x: torch.Tensor) -> torch.Tensor: """ Check collision in a batch of trajectories. :param x: Tensor of shape (batch_size, traj_length, position_dim). - :return: collsion costs on the trajectories. + :return: collision costs on the trajectories. """ assert self.cost_map is not None if x.device != self.device or x.dtype != self.dtype: From 0259b920429315892c186da8d2bb5d391e782e77 Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:09:15 +0900 Subject: [PATCH 21/48] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../aichallenge_submit/mppi_controller/mppi_controller/MPPI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py index 18ca6044..3e2dfd52 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -335,7 +335,7 @@ def forward( # use only N step optimal action seq optimal_action_seq = filtered_action_seq[-self._horizon :] - # predivtive state seq + # predictive state seq expanded_optimal_action_seq = optimal_action_seq.repeat(1, 1, 1) optimal_state_seq = self._states_prediction(state, expanded_optimal_action_seq) From 925af61a45a0770de214e9697a41427b39734c6a Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Sat, 31 Aug 2024 20:09:24 +0900 Subject: [PATCH 22/48] Update aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .../aichallenge_submit/mppi_controller/mppi_controller/MPPI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py index 3e2dfd52..d0fa4af7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -377,7 +377,7 @@ def get_samples_from_posterior( assert num_samples <= self._num_samples # posterior distribution of MPPI - # covaraince is the same as noise distribution + # covariance is the same as noise distribution posterior_distribution = MultivariateNormal( loc=optimal_solution, covariance_matrix=self._covariance ) From aa88f2ab39b5d8d4283fc2c44d788e072898b9ea Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 20:21:41 +0900 Subject: [PATCH 23/48] add some spell to cspell.json --- .cspell.json | 13 ++++++++++++- .github/CODEOWNERS | 1 + .../mppi_controller/.gitignore | 1 + .../__pycache__/MPPI.cpython-310.pyc | Bin 10093 -> 0 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 165 -> 0 bytes .../__pycache__/cost_map_tensor.cpython-310.pyc | Bin 1831 -> 0 bytes .../__pycache__/mppi_controller.cpython-310.pyc | Bin 7808 -> 0 bytes .../mppi_controller_node.cpython-310.pyc | Bin 6746 -> 0 bytes 8 files changed, 14 insertions(+), 1 deletion(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller.cpython-310.pyc delete mode 100644 aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/mppi_controller_node.cpython-310.pyc diff --git a/.cspell.json b/.cspell.json index 3c1715b8..7fef0397 100644 --- a/.cspell.json +++ b/.cspell.json @@ -104,6 +104,17 @@ "wpedantic", "xacro", "xyzrpy", - "zcvf" + "zcvf", + "mppi", + "Qdin", + "Kohei", + "MPPI", + "Savitzky", + "Golay", + "reparameting", + "tempature", + "rsample", + "coeffs", + "softplus" ] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 379992ee..13913d57 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -12,3 +12,4 @@ aichallenge/workspace/src/aichallenge_submit/racing_kart_description/** @Autumn6 aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/** @booars/aic2024-developers aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers +aichallenge/workspace/src/aichallenge_submit/mppi_controller/** @tamago117 diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore new file mode 100644 index 00000000..7e99e367 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore @@ -0,0 +1 @@ +*.pyc \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/MPPI.cpython-310.pyc deleted file mode 100644 index 624e444edbe033ca6540a9d6462d8a7a15a6e5de..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10093 zcmbtaON<*=cCA0Z&Hjk$pZqDwmMz*+%aZ?XjctwXamE8_6ic2-+Hq=1d_{J#i&fOC zVoT~NW`an@APEF`6(AV|VIx2gXOZt}Hpw~&5Cqu_@Q_U~D=o6iN>1E4_Z5p|cT0+q zu))js|J--qz31N7(acO)!Dr^)zqHnJndy&#%)5%B#v4d5WkX>q(*m`tZD=Z` z^+4|$8%EdMFuT@ zRi#nfsh7VOZF;`_D^bYY)Asq7&cD1?zE5jusG{PAVU)OuABAy47sdD7AaJh-o-nWV zd-yTl_nXOOG+emU4-$Xd<-VJEmm}VFgC>nJ{V8S{HNK04r);R$m&Vi$ooS4|1~WZl zXfkV8^^6Wy+bXiReyX%oX0zjWXRx;u?3ugDM$xCT3+xnY+%-2!>|5*w_TpV*qs-oB zYwRpi74{B0$Ic^FW$&_A*=tD6u#4#gwgCX_x=l%ENn*H--$w@`(Yv~VZZCdZWnBgMTz;yfbpKF zP_miLEsE@R=yv^P{NR=RC_eo(iFk8!?V1qx4UjE3W3|b&Rj>Btsru-KewMmPF5(4>roVlig&9Q zL|lrOnCZu!6Stj~A0!?ZH6(BPA&YK0vA^Sq8RUCWaNCJEL$wim9upS#l0FZGaXAXT z=8>G;_+7-egfDom#x>AHxuXo#T}4$AZK$UUyQ-?Bg^rOLEp+A(^I8W{F(B{+8vs49;M}! z>ME%QT1`>!%%lcrEj2-BQwwx1Eez*(mEi)uM`#3m%e&O0o)#Zbb2KeIqGmBIqvuju z1zk>OKv&Wl=xRC(dJOw|0^?WkMXCJC`IvE!R`eux^i*16<`HFA-M^a7lh7*G=h`%F z=@-D`&eIs_NqqB>XrFd+oE1=3xude-7zqJI{kY$4d98{Oe) zFU|SVWb8xzNG{H$I2q@BTKPidpQG)|!&k6=NL}YO%&^oZzKzG*&A9P8YTxL*iJI5j zDl4;!p2uEGvL^Htp!VPd43BGtFvbGh5KM+#|uS_zKI?7$1#$!4b6rn>aT>e7K0 z^Z2$K?CYKv+NWX>iSl6N2y3m<0ZL&Qn+lRIu>D~kFN2P~*j1)AQk5YD*?8>2rV@SFtFY>EI09@6>-~ho16JW zHtbKvrb8!;bJGj6Q}e_2p}zZ! z!@B(m)_&F9_LH5h+h;zE0{6C^*^NUT_nVP*SZ%T?nNZPIA3lm5xV#NP@?$&hMp3ef z?bw(4IHv2K9WnN>X(rYQ7XX$jjJiHxgS1zYtB;uKDzH*;V6KA~la8n9L8jy{Y`o3; zp^atPaTIJngx4&&-J|wy;@Xt_23^RauG?2ZsR=7v znK0#q_cb)Sd|w4cogPF@G2e_NDh!(*;6cLuTVJZ8n%zz(-gJ9j`^WXa{P5p*?!E&U zsRg|8mN4T!#7Pt#s1!eO03qaJM5XJ7eK&BV?KfrTY*|%5cHC{(m)Mj)M=LEj`O12n z;Faab6=KjFbyb}y)gj8x1aehndUVl5$;m+%8jWwQFq;8D2QeVQZ4p`$VGem$vtK}= z0-#HVg>u|eb*3(Xs5$$@TFesunFc*IDwxQXFn2s2#iD{`%8v zhuhOlOzwtlPZUt>@z~=QG3baBk_y09W1PzMK3#rcT$NaBghR9Q5e1$d7uM@a6_xdj zi3Px`V$s2A;qG7eSl%G^Zp5PQK&5Z{Q6Gns^+PdQ$D@nuwCUnvpldVYH|iN%5w^%5U9Y0^eiooUFag`p^2Au~tH ziG;H)tVDulF@ualN=&LSValxA z_s_b1bCYxj4)^TMh;PL`x9Od|-uDA`w%hCZ;HY$2pj{5-$?sX~-R9>oEiF49f|Oq@ z>vQ^?I;SohHMOeEsg`Q#_>TToRrC>=sZz=|E9EFgKMV>0y~Au21cW12*B2kQ$=2Sze>ULC8ykG&Xf z9x2>hrtM6oZJbQsgLa#k1bz~;^9IP}Mv=Y|PkG-Pn*kC#C~f3rAhOrw`PyjvfT=%`H7Xi8Mh{YfI;6 zQgv9$+wN)u>Q^3C;Mi1ml>xDSM#AY?rb*a7m%vHtEJ(-c$g)yb?x+KU+@#JNgS+;* zwxTR40}~LqP7YINE;X1eZD&pwf%`?26TDx-cUc1aqgcfXau}BYeP;(jtbZ0)BsM{kldG1yVT0ttq@m^X1VFo=}}RxFDZEsI5fWnjy;8b3p4}1Ip2&y z;gL1OzeB5BQ3j=Cvd$3ew3xYN@PDH@mz7Xu<^axK-ZL%dt3Xe3@bZ;UmsjCnz=gY@ zz(Lel0ly{u7V&H0w}fA1NbcM>=0&Q3=Y^a{Ee#{+sU6q9=33zo#QAgPWQO+ zuag2epN)lPAaoX81J;do82msKXc;rwE4gBtoigB9wm3dg7hd7^W8HZ4?W zWhaj*kPE7TTPa(@sT`7P3F(3SkSrdpkJ=1o(^)CSr{JFbB00u%DM4$4IkajFU&ulZ zofkW&hi6c-wyO^2G3Hr0*L*(HV1dCMMtFc+;+H5qu>{E<)YJO#9E~8yI!~!D)Ej?- zHZNym4PV(sopTboSHI9WdP`^awRE;kvW-2Ev)g%HI=eZC^ueN(p%ZeYiv!$};Tvf# z8B25?81GF;^y275P}sQ1ixsCUEe&e7w)4xe+PzBXR;t~Rc^`WvJsu$r*_~+J zi@Y7Lk2{iwDpjI=YC1^OfV0)-+zXSecGCT08A&=Y!vCs5*KOH^9LOp6b!2vJ_*SDu zJjzI%fyT`&1tl+MM5F=y7Nw*;e4A32L4bSigB98m{kU7e`^^SZ2X* z&?uZ5&C-O^*te8y!d!1g0Tc6n2&WR$vOMT34;7di!GKs)M~GSG_%@A}$p+k4?-oyJ zjy{!EBt{%bNe##rfmw)2Hp)T{ibtbC429~5(gf<(q(d{|QNioQJ{%ii1W^nKk86fp zdmpkFM>1b~f1&D&V_+p@9_0_2cRnwZj3YoRYI#w%yo1=M04PHT^*7jO{Ldf@<+8S@ zE(4a@8N2`$YkAms_OrV(IVRG<3VuqKdmS+A*ebs2v}tq8bDfRkEj>fC=@ zRuKSmLDRmT+gnCD2egK2{g+{_s>WWmprM7fw_GrE>SKLvm_^K2#>{oJU%+@ZqKk%R z0QhQOYliieQ7U6aHNf8mwZ1oJ)^w~;FCxE;@}jX*mH2J6$tJnZ`G)`;eI*$CO(Z1P zQnu8%Ac2@V%uubm3m`V2w3(7RCc~u56aZ7Ab}T?6yc^xi>Wl*=1gZ?Fa`l~Gj5X(n zgmWW=-Lj{MiH1C^V5j_N`xKmPXxFI#90AgN6be7Iy<{@j|6AQoEm7)OKy#nlBX@8z z&o$`AGV|i57c#_^8ih=ANu_9DjIB(;z%|A#_F}25L;=Acyrm=!GrQ5`>WKS!OizK^dOKt{v(*YV1PI$24B8IH#Ajvbvx_ZnT}cl!XJ0#awh)(!>0E{pM>Pt=7a0~d_ z$^_&xe*t50vR63|$=TxH1HqGzl$FfY`Iw4-KWisFnlGt9Dn-f1#U2zKG#`BWeW8a! z&d&Kp0Qbz5ie~J4C7=F*hO;l5z;pG-C2HrX8SGpMFAY}mY zB-pPEi+L;D)KM#ps%7cvGJvtscqqk;d22Wq&!o~@y(}SwL=(~jZIq^bPl*Sl{gt3# zV*jOKCYziKyF97CMu8}gHFYd)(}81)>n%K(!;0JC*aEOEJ@xU zHkhI^9qTGj;#~XqPp`%#YEqzkNTe!L7Zs-A)s9}$v@Mm_5`|zlUhwexy{s^G6}k}G z4m<;(2jr9|MA{K=%=e4|X+jfkZg_JBC-Kq@2i7F*I4nT$Rq0fb&Mc$_0Mep+8oL}T z$oHaduMgEm!GMQ`bb(P3(ej%s54cLLefyiEg#Bwyp}`S_)NBkI2$K3_xrcy z#d!A@rehTL##c}7AsU6u<3ztf}%>fV*`c5|8aBB5yQivN`78oOhdaa=e z^|C1YVG~`5qm7~{|G*9U-UpmVJVUt_qIUjMf@OUiqI=iF#4gda>7M5d~8zYAUQOPfbVJ)YHj@8`hCS8W{O{3mpU9KAlOC5MPfJap67E3kGqHy8hptX_^1z1Vc|q(+ z-M}l;CS`4uOF@U0$3IoTe`Y2;S8bBc1wS=W2CISn5Yjf- zI}&x0RKUh zMDOhX9*Vae8VUdGP@XL9pbY*VI{utwkYa?@+8P402-EJBjh)p@_8#WQG)`oJZ0XOD zY0f-58Pb+zGOjIC^HjS)ghIufBs~jrFH%azx@BsavNE+oDY6S$==%vu$@sS{lRXg&dz$Mz+%FdU<$qCc4iD`_Iv97XEd|!)sRAaquME4=7!AoEv!3 z8r2jWheb`t$)tr8pD4)xo_YDa4AgV7e|$*f&x!mM5yD!Wv=ApT5Hq7Uf~FS)7%8L* zNdihmL`b*LPGZ7zMa59Hc{nZdk4!k6qW@5SQmkv$>YnnG^11TS@{#hw!c($eqsIRU zy=W7yK0Wwi4vh*dJTpQsa~&B@2sAsQ^Hf*NVlZLIe?-~mv+Okdi`TkS|3HQZCi)Mu q?4ioS|7C$$a`vh1$UV_^MDaq#&bM&`GH`7uRih3^uZq7#{eJ+4y2-}? diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index bd4bef9f354ab13a6bfeb727b79715c2440c1371..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 165 zcmd1j<>g`k0^Z&WX(0MBh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o6v5KQS{oBQYl@ zH7`9?zdXMvySN}RIaNQYG&3hfKewPDGd?*#ucRm+sG2 SKczG$)edAsF%ytrVE_QkFDWem diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__pycache__/cost_map_tensor.cpython-310.pyc deleted file mode 100644 index 3f99c96c85732caae4e643e101ce192d77ce0711..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1831 zcmZux&1)n@6tAlO%p}>(W+lX3g<=mf4IzOQFGG-p90b95aPcrAO>I|Arf2%g>K@I8 z&P8+e>@{=rUl9B!Jl55N?4RJG;(OIIlMY%?ufBfo)vNb@ugcxs0fF|@N54(>144es zMZY<)cmcis0E8rxW@L;b%V^HVjFL}?bfkMjr27LKJCc4uBJT}g$$3PL_i|BXD#2yH zZNcIN^!iH>ij1iwV8e0_DpY|AChT1zeHlzb8Op62Z@eW3 z7UQFwxN=+W+_>X`48Vz9vwK$7&1a%|sfxPP5&Hu!vrW4&7&lo+VZsDJaV{#em8dL> z>-0()ztrhPS{PreW}%Do#9wLRfO8M}8T9%&h?ce_CM^R=Tj$zo*vx62)@?mWe{^8i zg*l16)&uEF2F^KRP6-+CRYM;*_`nMFZiuf46L2 zSEwNx!EuUi<2_v<@30XwerMKrR_8nlG-gr*>51`d(2U=yncdFmxLazh5WNi|@{L=o zY;vcmLp<96ekOVkHr{@IBGP0kvP=~h>g2mp&+AGg>g3fT&E!d5RcV}*MWZ3dO8;lQ z&gJU<@WzB@5}OIn=J@;{V=%9TJXk1gynE|bIzHs48Y8QO_Qh}cO= zEG`9%tef{?r``wgz(1sWbk8}U&)741;JEDS$$gUb)uL@=Xd!YH$7T@6c_|ke&UfPY zn}x`_6YIDw78BV2;lt;~@1k!!yP=V-8t>Qdp}=D53G`PxAdtv4B0|Ev5bD5v>oWEa zbJ*Vlm;J?p!JkhKEfRuZ!{x*80DowA0h;KUwYS(=d7hrv&dxpnf)EIhkTkMvg&U(liDK-8#Yk33e#ns^%aT1p#wB9DVmWsmifWu1cz8-g3#s2Q%N_v+t!%rYhwH zx~JdW)7}5~fB!#eqv2}!{rH9dXn*n-n)aWRnf#f^d<7wEBjOroiMCxn*|v_no){f- z+hkN`Bv!}XwpH3pDjjFrQE8S`x2uu1@9x*Qg=f93bDLN0o2P8M@pX+myn0{b)l+@D z$=L^5%YBHsw5=AC)`$IG5+Px}`>k)ijc4P{G|S%&dLKsJEEUogQQjBbkWq)hkJ1Lh zn+VxuM1_`X`|J|dPS0g+MdepD%%pEU)X}1)ODj)BxF_{qj9J*>r<$OB`Tcv>f;ij@ zk|gTxM%V78;^V9rgweH~ew^@Yon9~Y!?c@=6fMM)@|(RQxe#=BlgRI;q7x+Xaa5&M z(i$%7qK-$1F!gI7_Z5WfzYs;*HscyLo<+u)$@OzQ_pP?U4Q}3N+cvh>*spL~#g)jR zCxy1YrnPOg(dKrQKhJ0DbYA22`_{J08&Qo{c(ZNt1-^LS+^!GCyf7G(Mzi@6zlgfV zRNZpa#JE)N@Mri6>KCeO+#9sHtomPx7Wq2DaQ;QLGM7KgpTq1IcoT>6f?Sx{*sn=5 z?DHVBum!^(rD<=}G5-?|yb6ucv|DsASsr0f+UsUO5 zRC-yZS5*2LmA<6XD=NLpk@MF$jdNMWS2zjQ>XvBYaHJ9T`qJwAowy4jvw3t7hmo{+ ze$>MQ<-wt}vv{`?WU{iCiugF~%38PI@w1=<(a#qyHbBYL`V=05K?KpA9tnsK99SCkf*gSZOq$_ck!0dPJdTa zXb!a*^*Hl`gCI_V9iYVpHIYBq65B8L#LgA6%`8t4pow+r(HOl#9%HZUEKLp~A)coO z9z`!u^f`)Nr0DY$eSxBvC<4Z7aw+T!5p{FF7vy_>+~v`sY>KEIp)ib8sdUslouJpU zq?tuYTU@37EsALAVu>OG$ug=TB^@85#kud(`(|$;vYE|R^(L$9He1)%**en=hdE5g zpE>%Y66b%`7pT5&)Y!ibz72m4 z5(KvjF@A*v-D98z5Mv)0un){YY;s078&6=SdIAf80hR9&vRDYz*5Xzl+nU?lK4m{- zdF8BfuANYSLJ-C)57_tg6Z6Cx)Yyex7^~Vz1ud%Lo4L!KGeU+6uP$rcJp~H!+NsVo z;E>hUAwl)v-*BH?Z}9t0kKxZxe)Ke)Hy9o(53 zdHz(q*=foqUU_h8siVQSHoR9ShaZpqZKd^RUPDoZ+mmmx;Z5f5-1LHwWX8*)yM4Oo zUMXv%2?zMI;a$hHK^R5}s`4ljcj!sm|Ae027rj?Dyc?5?p4tO49V>U@r=| z&tnXWg~&^z(;A8-uJJ*Tv<&fOTvYMb6#WgNt>X_yO$WP`?qCo=lm}VeP)DBxU6u1X z{Vex(Xxa2=rvjG;Tn3WjDcJBjQNEY*%@z}Xg~5-TWoaIFqMP0iPU7D|d=&N`Ti)$X zcBj>l7coHSV-0>Tg8eAO4j+k^X?fQW0RRS@Sfa%%ROWsZB>m{CLV&QDG`6f@Dfa@w z6`Dw^C*p1{Z4`EbTv{UScX`VZuhOWrF7YKAhD2TD(k&0s$5F_tIu(^PqAr)NTE9j%x!Q3WbTA^?fu@ivb+3&n{-Np|r~b znu1=^6{N$}T6VCLv`d_QqH~=H;28)PH_vqd=&;v0LO{;)~MW`Dz|fXrWc^e z^K;;oj1jbBtaIa}GSzx&X4TppC;~ju+R3fLI%OwTL9^I}bq<^2q^fe2LC!5ICpA^# z6b{uB>|fDN>V;7#y0@%d(oPxxdm?_W$~9@+g2D23ZbI?sCl`tfxPMOYv&Se9k?y4V0L^xSEaDyjk;iSN`|~(RJTBsc z$Qx3o|GVN)GU@FEc?i{{$jk^}RiU53?3sji=(IeT!b#Ff1FW9a7X#(@sPouokybw`*%VBhnAMGAYp+u0K|&ZKs^IaG;%;vL7sZ7Ht!@DY9waY!Y|J|r~x zlt}~J6-a9y#L+#)9;Fq6o%SX;Az6yx)@de<4D{207AeV}qJb}zwRo?k2@w4|nuqw0 zWJfS}Y11V;kd{((gsxC@Qs;*VM+gUC>t!RW8{P8{rFtC6D&;0B&`v~IKgnm{L$TEW z(`Cd!SDUMh{xv7(#$Jk1+kLTSd`b{di?F#HjHA0f)WU=Is} zh}(&YC!`w0OFL7fuCTa4*Z?z+K|V}ij=~T~kd8W)Ue%W26F5`!?J?~YwhY>~Q;>0iS?VXKqnHXJ<84LPjbO1_(!~MnT3j*0JA&2gi9p$kzn=X!{GG z(dKZDaqX9>-YIIAw4#B~#CjKs1)x(kcm;dR{G!gGYB*Yd)?;&a}mX_y-R6smTd_WNy zhSKPz_r}(ttoFMZ7;$tQDNZ2kVcO~S^GGRwSsB)ij7We^zoRTkOfqI^3Sc6dq)pK- z&7?=LmZL;gm8D5%EB>C!odX|i4kSi44x%ItzAU7GYp@JqpZRxXH;%*)|#0SU7E72lCafQjh$Nmd=miFK-< z=s8?f(D7Aha7Yd}ARP?M1RSHSBL&7oM&<+K#3h!2Ru%9Wn47>a_)bfyg&eE&8qf#~ z%6R}Y^2~uDTFa|o8Zan{g`CuLx18aWy`|lG5t0nU6Ovia$pozz))noPVJ%It4M=EF z{a6z_Sf|Q?`?y8pmUj26n7=xabBmfXRNdg;K{H1;bS%jbtqCi1Y%X{p`jgh=raS8V zv||4MVVu(W&6}e`C&-*BI))5o>6W+GpD}ppHh5RTASMiG6h(*Lv;j?|i?)Z>FnW1n zJp=1$I0l5B$EDFasF`a0);nVY10|d$eP!Pc=9(LP-tZiEADW^tnLTRfDGPOKwvpjE zZKICI7%`{80!AIm_Z$uOSPXoe`Hlf$wU39HxI;6{a=34gGr&hjwE=GTlxHzzNs()d zq?rITTY=i5?+69Sv<7L!H;{oes&62ZC6G#JS}<}*JJ}-zy8<3^W}v?bZEF<9nRSjf z(26F?loJ{bXYd$cfTmp0RfPd?J8^O?Tim{d_Fx`v4$I&a_RT6{NcHP*Eg1!z%x87T zKX8WIaCm=o_ic>Z07@{FU>=NGg^d6w7p)yUsfwB^^ftb{kRzN%fVEN`=cyN!VT~@3 z09FtFoJ;6^OQRHU#J>A>VUfkLRq7z(W#S!*ND+Oim##wPG{ojVS}4h_N#8;M zq)`l~YMx39r~aiIB0&>rK_!9YTFTut9tJ)Fd`5|+fzLAFhI!PH4!j!b(~O8|?3y^j zLpe{#Eeu)!H%lg6@+|jK%#g`?p5{R^%sE7B6{M3>LF9f6Cd^{l%%a^6xk~8^P`+1c zwTtSe_fjG(d@#Zc{gn*}q;g@aQGbKgWX~bGuCKtu`5c02EW^)9o={KUV2jX7&__4t z^wGS#W%CvkW{_mBZB9*5?mca=_yM9VMGe#msIzD_#3|L$MHc@+(LYi|mU-!lwxN&V zrWXa3+(#r0c<03rRSGXG-arb6Hdgp*i;tDB2$4WO$`yz{^c?dC<1_V&r z5MwBP@e^ub!g0=L_Dp?bId;Qvb<=*9a zH!u!-X$N?SdlVf~!4mna@nsy2-Wdbfv7KI*~Gr&cGIXLpId zrmAlWa?Y`&2oM}Teu=Q<`}{4|B#r$TUb-w4aW5CYM2Y%fsTT0KHbb)V^-ot>Tiz~GA{0D^=_isH&Lpbxu}IEqM>6v?Sblo&FOv#~NBY!4PQ zoZVUV%#y+ys2s?YeL+=HIpoVu0j?xfy!h;#>{R{%R}MM!DXAP%ELAE_=*WtqnEAb) z+1&vMIh93C_j|8jf4zSFJ{obOVQcvP^-F)h_2Csw`)5LmKLf}$e94bcaEc#dV>S}bVce;IA@!HXu-cox>*WT24jo0sMyzaAm zrS>v+c!M|Z>+Kc)ET6f}#FoEuPeUL5rutg553wuldP|qa^_cspt#^3b^HZ_YDoNwr zIPqoq-8kr{(!MeDQeX6gxZh&Zz9qcS@1(KVkuy_Rv0{|duXb+xqUZJduf_dT#L;VU z4;V*Nn_1yKbqI8PxS}YKGQs+|*i*oO>JV%QuDPk<>bRVL1%2iH}y5d8800|ZxoC*O<=5uTU|E6)oKbj*2J3ye1@*@@Oj>tz-xuMo+_;80>4N+0qzO*S>o(aKQIopu>40l zHx$JvP)tQJbBYO`s-jd2l)9qS3zQi}naL>?^qN(a*_>j5GOsA}1)c>(Stw9WDaxq= z<&2`7DNvRaWvM_}R+QxeWkpd|3Y1kvSuIe`E6Vu-Wld4mrWA!gHO3WoVT|!h-)4WM z@9Cgc548iEKfSB}x%M4MSLstNt@F$LnZweq<~G1nOPl;kj+3+qZ{@fHd^Vlq&+cmB zJZLx-mt->wp3i}{nA1o)v*~I69B5~9+ETh4t{j{_)c!yh!*uyzm4E(F?;8qQKg8QI(f4%hUV?@LWEG#<vG|k zIdJ>9aI&%;i(nM@A1>jR$y$Hda}%#Oi2Nitm;t3cbbCR+i$?nWFb9701J&StFmAXu zZn!GVBkrqU;S!6<=V9t6Bn>;aS_GH zf+^h!x?frw&BeoXFibac+v;|D{2^Jc(R|)8h+waMu9i8nxxYE=ZqSlPPSB^0w}Zal z^U|P`jFvOXbZhCv9gG@TTL(tF=MALM8FEjSI)mY@mLbh-IkK#VmnQe`1ReMop6(3% z?)T6A{)acd{})$VS7bRE4tyak7Xk}X*Ojv{w2>!Bio%13LSv)rrv-HW2&=c~%2uPX#)nge1Mh5$r-`*5B(8VI3nC8!5-lQCwxZZeUwTnI z3ry6BWasJw7HP$KtRU^_ys|PLyq<3|4SI^X?`BY0RR|)C#E-VbGZ^-PE~~DK^Gy=h zeV~1EYr_jV+t|L}@A?~eV{tnfcpZOZa~MQ?byHtp zqm?J)j`hjiDD{$Vha!*hk6`*YaKDvCu^izeLXxC>hLfoKHe**<3e}mwgR2-Z2SBOU6EfgotIn}fO=bUfS zuqVxV6%E75oEJ~n=kY6aB;KP-JC=0vWgryqDG#l~Loa+PSh>KvPDriBN8#=taU*4m&tR9@(#X2bf1biNapw4Ch1)s)jC%#6 zw+^1&Rf$ddIVe5kd+;1)_&mP#Lv7a%H{9oWP1P?Tr6L(wNRmh5 zyO#H+`URvunS@uvm&U7BBh}PMU%*H;8cBtX&Fc^8Mj{MSZi_tG5nWOy4MegA2^qgG zWcS`BTv|QULOrBQN_8nU4$2BPbD9bLqBhd4thSVnXwVTotagn*WbZRvU0rw>wq;9W zKpC`!GEmn~|3K`D5DnITWVGs@BIAJkj{B z8lN(FH^XEfn~z5uC*0XM_u}g>V_q*xUI8#3w^f!1HHia~K!R6QhXDlVarC5-XJsX1t&Q+xE6DAcK4$F0{=-24p-J@lbz*!K1zenZ^-%$fP$KA1+f z=x(R7d^h0fwzSb0M?)2@t2pI#VyY+PmB)H-4hI;>mowz6(|9n>OvPp5EhAS&BubuA zw5j+VD!xX=dsNUHmiPq}Dy5(lL!}hrD@0c=`K#23b$J}g$8Qs2ZBatx23-6iq0MPl zM%kPAB@kQnEYx#}p^j()DNH6&o}A70I<##!@!yx#Yybjlg&nFe1Uok~ zR#`ui0%ZxMN`H>4;ICm-_wXfs6v`SQLZ*n^m_+57U8-uPEoLQ{}$yz^7 z*QQtF5~jwZ=ps%wPE(|^e3->{IGWBIqm^ld{6yc;nfy?nXt~uYXE*E%7+tA;7Npa~ zfSp!p!ZEw?4Kz`Jnk=9&^*J0?u|&wXeuDe>IhVfGq-?)Ws_aVJb$c;~O9WhZ$s~^TUfzlj7(Y*LN6{J*x7AYHfW~m&KTTs5Yh()D4O+e&a5-KdM&YJHv zOU;s1vT6<&)Oti0D1snYrF7v6fE4kcd{jCg=I9xK@%raZojhj#I8j4lm&^(ri!)+~*l7xW(iO2`{P&2<~f zF1|}`5G1v_ZkBSbitiBBR`ac^!DtJz-uLcM0+Fnv$lf1jyMFfmKc$n_D?g(gj66^8 zUw7#xMm>b7OHO&jWSk1|)0*VDqpahXrwM13IO*JEHCx*IaUyDjV{=e&@&MvngtPu2 z^K9pPNB=*tehklEDW2dOo9EoTWoDnwRmn^I2|-f@SCA62^>U_$uh8{R=%}F`5c7hv VECh`sCxRWO{hrM&)7Fj0{|l9Y2e1GD From ea112e4d9ac4f3a7aed94b37802251b306cab63d Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 20:25:13 +0900 Subject: [PATCH 24/48] fix typo --- .cspell.json | 10 ++++++++++ .../mppi_controller/mppi_controller/MPPI.py | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/.cspell.json b/.cspell.json index 7fef0397..b31b79d0 100644 --- a/.cspell.json +++ b/.cspell.json @@ -111,6 +111,16 @@ "MPPI", "Savitzky", "Golay", + "Vandermonde", + "michikuni", + "eguchi", + "vander", + "pinv", + "dtheta", + "cind", + "dind", + "ncourse", + "argmin", "reparameting", "tempature", "rsample", diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py index d0fa4af7..90f52793 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py @@ -323,7 +323,7 @@ def forward( dim=0, ) - # appply sg filter for each control dimension + # apply sg filter for each control dimension filtered_action_seq = torch.zeros_like( prolonged_action_seq, device=self._device, dtype=self._dtype ) From 202e565327ed579b20160b035e07103ba03a13c2 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 20:26:19 +0900 Subject: [PATCH 25/48] add some spells --- .cspell.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.cspell.json b/.cspell.json index b31b79d0..7978a84c 100644 --- a/.cspell.json +++ b/.cspell.json @@ -110,6 +110,7 @@ "Kohei", "MPPI", "Savitzky", + "satitzky", "Golay", "Vandermonde", "michikuni", From 4b0f44a642c3b384818f06808b475343e6ce8684 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Sat, 31 Aug 2024 21:47:34 +0900 Subject: [PATCH 26/48] mppi + pure pursuit --- .../config/planning/costmap_generator.param.yaml | 2 +- .../booars_launch/launch/components/control.launch.xml | 7 ++++--- .../mppi_controller/config/mppi_controller.param.yaml | 8 ++++---- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index 761d16e7..c5c54ba6 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -15,4 +15,4 @@ predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" - distance_threshold: 1.0 + distance_threshold: 1.2 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index f23f0812..d39ec7b6 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -5,12 +5,13 @@ - - + + - + + diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml index 15fa92cd..8f0d35a0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -5,16 +5,16 @@ num_samples : 3000 u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad) u_max : [2.0, 0.25] - sigmas : [0.5, 0.1] # sample range + sigmas : [0.5, 0.15] # sample range lambda : 1.0 auto_lambda : false # reference path DL : 0.1 - lookahead_distance : 3.0 + lookahead_distance : 0.3 reference_path_interval : 0.85 # cost weights - Qc : 2.0 - Ql : 3.0 + Qc : 20.0 + Ql : 1.0 Qv : 2.0 Qo : 10000.0 Qin : 0.01 From 5d1e2be50cdf7c3feecc1351dc65f9c63cd00bd7 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 4 Sep 2024 23:11:46 +0900 Subject: [PATCH 27/48] chore: run pkill when run evaluation (#26) * update pkill_all.sh Signed-off-by: Autumn60 * call pkill_all.sh in run_evaluation.bash Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- aichallenge/pkill_all.sh | 2 ++ aichallenge/run_evaluation.bash | 3 +++ 2 files changed, 5 insertions(+) diff --git a/aichallenge/pkill_all.sh b/aichallenge/pkill_all.sh index aeb66d32..e2c07d9f 100755 --- a/aichallenge/pkill_all.sh +++ b/aichallenge/pkill_all.sh @@ -1,4 +1,6 @@ +pkill -9 AWSIM.x86_64 pkill -9 component_conta +pkill -9 dummy_objects_p pkill -9 ekf_localizer pkill -9 empty_objects_p pkill -9 goal_pose_sette diff --git a/aichallenge/run_evaluation.bash b/aichallenge/run_evaluation.bash index 67045b47..90d9c142 100755 --- a/aichallenge/run_evaluation.bash +++ b/aichallenge/run_evaluation.bash @@ -1,6 +1,9 @@ #!/bin/bash AWSIM_DIRECTORY=/aichallenge/simulator/AWSIM +# Kill all processes +source /aichallenge/pkill_all.sh + # Move working directory OUTPUT_DIRECTORY=$(date +%Y%m%d-%H%M%S) cd /output || exit From 43182d17f4826aace39113320e01fa2bbf86aa7f Mon Sep 17 00:00:00 2001 From: sitahara <33857284+sitahara@users.noreply.github.com> Date: Wed, 4 Sep 2024 23:13:45 +0900 Subject: [PATCH 28/48] feat(control): stanley control (#25) * feat(control): stanley control * chore: fix spelling * chore: add myself in CODEOWNERS file * Update aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp Consistency of indent Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> * fix: fix parameter names on launch and code * feat: add parameter descriptions --------- Co-authored-by: Shotaro Itahara Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .github/CODEOWNERS | 1 + .../launch/components/control.launch.xml | 15 ++ .../stanley_control/CMakeLists.txt | 20 ++ .../stanley_control/stanley_control.hpp | 90 ++++++++ .../stanley_control/package.xml | 26 +++ .../stanley_control/src/stanley_control.cpp | 196 ++++++++++++++++++ 6 files changed, 348 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 13913d57..1cf6e6e9 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -13,3 +13,4 @@ aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/ aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers aichallenge/workspace/src/aichallenge_submit/mppi_controller/** @tamago117 +aichallenge/workspace/src/aichallenge_submit/stanley_control/** @sitahara diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index d39ec7b6..6458d746 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -14,5 +14,20 @@ + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt new file mode 100644 index 00000000..6d33c02c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.8) +project(stanley_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_executable(stanley_control + src/stanley_control.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp b/aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp new file mode 100644 index 00000000..9ff5f6cd --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp @@ -0,0 +1,90 @@ +// Copyright 2024 Booars +// +// 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 STANLEY_HPP_ +#define STANLEY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace stanley_control{ + using autoware_auto_control_msgs::msg::AckermannControlCommand; + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Pose; + using geometry_msgs::msg::Twist; + using geometry_msgs::msg::Vector3Stamped; + using visualization_msgs::msg::Marker; + using std_msgs::msg::Float64; + using nav_msgs::msg::Odometry; + + class StanleyControl : public rclcpp::Node { + public: + explicit StanleyControl(); + + // subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + // tf2 listeners + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener; + + // publishers + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_angle_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // timer for control + rclcpp::TimerBase::SharedPtr timer_; + + // updated by subscribers + Trajectory::SharedPtr trajectory_; + Odometry::SharedPtr odometry_; + + // stanley parameters + // speed control + double speed_proportional_gain_; + double external_target_vel_; + // stanley parameters + double k_gain; + double k_gain_slow; + + private: + void onTimer(); + bool subscribeMessageAvailable(); + + + // control variables + double error_distance; + double track_yaw; + OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters); + }; +} // namespace stanley_control + + +#endif //STANLEY_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml b/aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml new file mode 100644 index 00000000..9e64164d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml @@ -0,0 +1,26 @@ + + + + stanley_control + 0.0.1 + Stanley control for the people + Shotaro Itahara + Apache-2.0 + + ament_cmake + + rclcpp + autoware_auto_control_msgs + autoware_auto_planning_msgs + motion_utils + geometry_msgs + std_msgs + nav_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp b/aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp new file mode 100644 index 00000000..7bf7d936 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp @@ -0,0 +1,196 @@ +// Copyright 2024 Booars +// +// 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 "stanley_control/stanley_control.hpp" + +#include +#include + +#include + +#include + +namespace stanley_control{ + + using motion_utils::findNearestIndex; + + StanleyControl::StanleyControl(): Node("stanley_control"), tf_buffer(this->get_clock()), tf_listener(tf_buffer), + speed_proportional_gain_(declare_parameter("speed_proportional_gain", 2.14)), + external_target_vel_(declare_parameter("external_target_vel", 10.0)), + k_gain(declare_parameter("k_gain", 2.0)), + k_gain_slow(declare_parameter("k_gain_slow", 1.0)){ + pub_cmd_ = create_publisher("output/control_cmd", 1.0); + pub_marker_ = create_publisher("debug/forward_point", 1); + + pub_angle_ = create_publisher("output/angle", 1); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); + sub_trajectory_ = create_subscription( + "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); + + + external_target_vel_=10.0; + speed_proportional_gain_=1.0; + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer(this, get_clock(), 10ms, std::bind(&StanleyControl::onTimer, this)); + + // dynamic reconfigure + auto parameter_change_cb = std::bind(&StanleyControl::parameter_callback, this, std::placeholders::_1); + reset_param_handler_ = StanleyControl::add_on_set_parameters_callback(parameter_change_cb); + } + + AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp){ + AckermannControlCommand cmd; + cmd.stamp = stamp; + cmd.longitudinal.stamp = stamp; + cmd.longitudinal.speed = 0.0; + cmd.longitudinal.acceleration = 0.0; + cmd.lateral.stamp = stamp; + cmd.lateral.steering_tire_angle = 0.0; + return cmd; + } + + void StanleyControl::onTimer(){ + // check data + if (!subscribeMessageAvailable()) { + return; + } + + // create zero command + AckermannControlCommand cmd = zeroAckermannControlCommand(get_clock()->now()); + + // Check if goal is "reached" + size_t closet_traj_point_idx = findNearestIndex(trajectory_->points, odometry_->pose.pose.position); + if ((closet_traj_point_idx == trajectory_->points.size() - 1) || (trajectory_->points.size() <= 5)) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal"); + } + // Otherwise (main control) + else { + // get closest trajectory point from current position + TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); + + // longitudinal control + double target_longitudinal_vel = external_target_vel_; + double current_longitudinal_vel = odometry_->twist.twist.linear.x; + + cmd.longitudinal.speed = target_longitudinal_vel; + cmd.longitudinal.acceleration = + speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); + + // calc lateral control + //// calculate track yaw relative to the vehicle + + //// calculate track yaw angle and publish it + if (trajectory_->points.size() - (closet_traj_point_idx+1) <= 3 ){ + cmd.lateral.steering_tire_angle=0.0; + } + else{ + // obtain closest point and next point on the trajectory + size_t next_closest_traj_point_idx = closet_traj_point_idx+1; + TrajectoryPoint next_closet_traj_point = trajectory_->points.at(next_closest_traj_point_idx); + + // calculate track "yaw" angle + //// Trajectory vector + double x_track = next_closet_traj_point.pose.position.x - closet_traj_point.pose.position.x; + double y_track = next_closet_traj_point.pose.position.y - closet_traj_point.pose.position.y; + + double norm_track = sqrt(pow(x_track,2) + pow(y_track,2)); + + //// vehicle heading vector in "map" frame + //// obtain this by transforming constant vector in "base_link" frame to "map" frame + Vector3Stamped vector_fixed, vector_vehicle; + double norm_vehicle=1.0; + { + vector_fixed.header.frame_id = "base_link"; + vector_fixed.vector.x = 1.0; + vector_fixed.vector.y = 0.0; + vector_fixed.vector.z = 0.0; + } + try{ + auto transform = tf_buffer.lookupTransform("map","base_link", tf2::TimePointZero); + tf2::doTransform(vector_fixed, vector_vehicle, transform); + norm_vehicle = sqrt(pow(vector_vehicle.vector.x, 2) + pow(vector_vehicle.vector.y, 2)); + } + catch (tf2::TransformException &ex){ + RCLCPP_ERROR(get_logger(), "Could not obtain transform from map to base_link: %s", ex.what()); + } + + // calculate the cosine of two vectors, from the inner product + // use outer product to calculate signed angle (if v1xv2 >=0, then v2 is "to the left" of v1 ) + double ip_vector = x_track*vector_vehicle.vector.x + y_track*vector_vehicle.vector.y; + double cos_vector = ip_vector / (norm_track * norm_vehicle); + double angle = acos(cos_vector); + + double xp_vector = x_track*vector_vehicle.vector.y - y_track*vector_vehicle.vector.x; + if(xp_vector>=0) angle*=-1; + + // calculate positional error from nearest trajectory point + + error_distance = sqrt(pow(closet_traj_point.pose.position.x-odometry_->pose.pose.position.x,2)+pow(closet_traj_point.pose.position.y-odometry_->pose.pose.position.y,2)); + if ( + (closet_traj_point.pose.position.x-odometry_->pose.pose.position.x)*vector_vehicle.vector.y + - (closet_traj_point.pose.position.y-odometry_->pose.pose.position.y)*vector_vehicle.vector.x >= 0){ + error_distance*=-1; + } + track_yaw = atan2(k_gain*error_distance, odometry_->twist.twist.linear.x+k_gain_slow); + Float64 pub_angle = Float64(); + pub_angle.data = angle; + pub_angle_->publish(pub_angle); + cmd.lateral.steering_tire_angle=angle+track_yaw; + } + } + pub_cmd_->publish(cmd); + } + + bool StanleyControl::subscribeMessageAvailable(){ + if (!odometry_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available"); + return false; + } + if (!trajectory_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available"); + return false; + } + return true; + } + rcl_interfaces::msg::SetParametersResult StanleyControl::parameter_callback(const std::vector ¶meters){ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "k_gain") { + k_gain = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "k_gain changed to %f", k_gain); + } else if (parameter.get_name() == "k_gain_slow") { + k_gain_slow = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "k_gain_slow changed to %f", k_gain_slow); + } else if (parameter.get_name() == "external_target_vel_") { + external_target_vel_ = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "external_target_vel changed to %f", external_target_vel_); + } + else if (parameter.get_name() == "speed_proportional_gain_") { + speed_proportional_gain_ = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "speed_proportional_gain changed to %f", speed_proportional_gain_); + } + } + return result; + } +} // namespace stanley_control + +int main(int argc, char const * argv[]){ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From bb4976392458df0c999472b89a7833dc6b95abc5 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 4 Sep 2024 23:25:42 +0900 Subject: [PATCH 29/48] feat(planning): create cached lanelet costmap (#24) * create cached_lanelet_costmap pkg dir Signed-off-by: Autumn60 * update booars_utils/nav Signed-off-by: Autumn60 * implement cached lanelet costmap Signed-off-by: Autumn60 * add cached_lanelet_costmap to booars_costmap_utils Signed-off-by: Autumn60 * update booars_costmap_generator Signed-off-by: Autumn60 * update costmap_generator.param.yaml in booars_costmap_generator Signed-off-by: Autumn60 * update costmap_generator.param.yaml in booars_launch Signed-off-by: Autumn60 * Update aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml --------- Signed-off-by: Autumn60 --- .../planning/costmap_generator.param.yaml | 15 +- .../nav/occupancy_grid_parameters.hpp | 20 ++- .../booars_utils/nav/occupancy_grid_utils.hpp | 21 ++- .../config/costmap_generator.param.yaml | 17 ++- .../src/costmap_generator.cpp | 2 +- .../booars_costmap_utils.hpp | 7 +- .../costmap/booars_costmap_utils/package.xml | 1 + .../cached_lanelet_costmap/CMakeLists.txt | 16 ++ .../cached_lanelet_costmap.hpp | 75 +++++++++ .../cached_lanelet_costmap/package.xml | 25 +++ .../src/cached_lanelet_costmap.cpp | 144 ++++++++++++++++++ 11 files changed, 325 insertions(+), 18 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index c5c54ba6..613dd9d7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -1,17 +1,24 @@ /**: ros__parameters: - update_rate: 10.0 + update_rate: 20.0 map_frame_id: "map" costmap_center_frame_id: "base_link" costmap_width: 50.0 costmap_resolution: 0.2 multi_layered_costmap: layers: - - "lanelet_layer" + - "cached_lanelet_layer" - "predicted_object_layer" - lanelet_layer: - type: "lanelet" + cached_lanelet_layer: + type: "cached_lanelet" map_topic: "/map/vector_map" + costmap_topic: "~/debug/cached_costmap" + cached_costmap: + min_x: 89607.0 + max_x: 89687.0 + min_y: 43114.0 + max_y: 43194.0 + resolution: 0.1 predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp index b6ccaddf..8c32ac5c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp @@ -25,37 +25,49 @@ class OccupancyGridParameters using SharedPtr = std::shared_ptr; static OccupancyGridParameters::SharedPtr create_parameters( - const double width, const double resolution) + const double width, const double height, const double resolution) { - return std::make_shared(width, resolution); + return std::make_shared(width, height, resolution); } - explicit OccupancyGridParameters(const double width, const double resolution) + explicit OccupancyGridParameters(const double width, const double height, const double resolution) { width_ = width; width_2_ = width / 2.0; + height_ = height; + height_2_ = height / 2.0; resolution_ = resolution; resolution_inv_ = 1.0 / resolution; grid_width_2_ = static_cast(width * resolution_inv_) / 2; grid_width_ = grid_width_2_ * 2; - grid_num_ = grid_width_ * grid_width_; + grid_height_2_ = static_cast(height * resolution_inv_) / 2; + grid_height_ = grid_height_2_ * 2; + grid_num_ = grid_width_ * grid_height_; } double width() const { return width_; } double width_2() const { return width_2_; } + double height() const { return height_; } + double height_2() const { return height_2_; } double resolution() const { return resolution_; } double resolution_inv() const { return resolution_inv_; } int grid_width_2() const { return grid_width_2_; } int grid_width() const { return grid_width_; } + int gird_height_2() const { return grid_height_2_; } + int grid_height() const { return grid_height_; } int grid_num() const { return grid_num_; } private: double width_; double width_2_; + double height_; + double height_2_; double resolution_; double resolution_inv_; int grid_width_2_; int grid_width_; + int grid_height_2_; + int grid_height_; int grid_num_; }; diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp index 61af7876..933f3917 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp @@ -29,7 +29,8 @@ OccupancyGridParameters::SharedPtr create_occupancy_grid_parameters( const OccupancyGrid::SharedPtr occupancy_grid) { const double width = occupancy_grid->info.width * occupancy_grid->info.resolution; - return OccupancyGridParameters::create_parameters(width, occupancy_grid->info.resolution); + const double height = occupancy_grid->info.height * occupancy_grid->info.resolution; + return OccupancyGridParameters::create_parameters(width, height, occupancy_grid->info.resolution); } OccupancyGrid::SharedPtr create_occupancy_grid( @@ -37,10 +38,10 @@ OccupancyGrid::SharedPtr create_occupancy_grid( { OccupancyGrid::SharedPtr occupancy_grid = std::make_shared(); occupancy_grid->info.width = parameters->grid_width(); - occupancy_grid->info.height = parameters->grid_width(); + occupancy_grid->info.height = parameters->grid_height(); occupancy_grid->info.resolution = parameters->resolution(); occupancy_grid->info.origin.position.x = -parameters->width_2(); - occupancy_grid->info.origin.position.y = -parameters->width_2(); + occupancy_grid->info.origin.position.y = -parameters->height_2(); occupancy_grid->data.resize(parameters->grid_num(), value); return occupancy_grid; } @@ -50,7 +51,7 @@ void update_origin( const geometry_msgs::msg::Vector3 & translation) { occupancy_grid->info.origin.position.x = -parameters->width_2() + translation.x; - occupancy_grid->info.origin.position.y = -parameters->width_2() + translation.y; + occupancy_grid->info.origin.position.y = -parameters->height_2() + translation.y; occupancy_grid->info.origin.position.z = translation.z; occupancy_grid->info.origin.orientation.x = 0.0; occupancy_grid->info.origin.orientation.y = 0.0; @@ -66,7 +67,7 @@ tier4_autoware_utils::Point2d index_to_point( return { index_x * parameters->resolution() - parameters->width_2(), - index_y * parameters->resolution() - parameters->width_2()}; + index_y * parameters->resolution() - parameters->height_2()}; } std::vector get_index_to_point_table( @@ -80,6 +81,16 @@ std::vector get_index_to_point_table( return table; } +int point_to_index( + const OccupancyGridParameters::SharedPtr parameters, const tier4_autoware_utils::Point2d & point) +{ + const int x = + static_cast((point[0] + parameters->width_2()) * parameters->resolution_inv() + 0.5); + const int y = + static_cast((point[1] + parameters->height_2()) * parameters->resolution_inv() + 0.5); + return y * parameters->grid_width() + x; +} + } // namespace booars_utils::nav::occupancy_grid_utils #endif // BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml index 8173e2a4..3f461d36 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml @@ -1,14 +1,25 @@ /**: ros__parameters: - update_rate: 10.0 + update_rate: 1.0 map_frame_id: "map" costmap_center_frame_id: "base_link" - costmap_width: 20.0 - costmap_resolution: 0.1 + costmap_width: 50.0 + costmap_resolution: 0.2 multi_layered_costmap: layers: + - "cached_lanelet_layer" - "lanelet_layer" - "predicted_object_layer" + cached_lanelet_layer: + type: "cached_lanelet" + map_topic: "/map/vector_map" + costmap_topic: "~/debug/cached_costmap" + cached_costmap: + min_x: 89607.0 + max_x: 89687.0 + min_y: 43114.0 + max_y: 43194.0 + resolution: 0.1 lanelet_layer: type: "lanelet" map_topic: "/map/vector_map" diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp index aa86eee8..60b34007 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp @@ -44,7 +44,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & options) double costmap_width = this->declare_parameter("costmap_width", 10.0); double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); costmap_parameters_ = - OccupancyGridParameters::create_parameters(costmap_width, costmap_resolution); + OccupancyGridParameters::create_parameters(costmap_width, costmap_width, costmap_resolution); index_to_point_table_ = booars_utils::nav::occupancy_grid_utils::get_index_to_point_table(costmap_parameters_); } diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp index dc4ef26d..dabb347c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp @@ -15,12 +15,14 @@ #ifndef BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ #define BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ +#include #include #include #include namespace booars_costmap_utils { +using CachedLaneletCostmap = cached_lanelet_costmap::CachedLaneletCostmap; using LaneletCostmap = lanelet_costmap::LaneletCostmap; using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap; using PredictedObjectCostmap = predicted_object_costmap::PredictedObjectCostmap; @@ -36,7 +38,10 @@ MultiLayeredCostmap::SharedPtr create_multi_layered_costmap( std::string layer_namespace = costmap_namespace + "." + layer; auto type = node.declare_parameter(layer_namespace + ".type", std::string()); - if (type == "lanelet") { + if (type == "cached_lanelet") { + auto cached_lanelet_costmap = CachedLaneletCostmap::create_costmap(node, layer_namespace); + costmap->add_costmap_layer(cached_lanelet_costmap); + } else if (type == "lanelet") { auto lanelet_costmap = LaneletCostmap::create_costmap(node, layer_namespace); costmap->add_costmap_layer(lanelet_costmap); } else if (type == "predicted_object") { diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml index 66e55ae9..fef8a5ab 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto + cached_lanelet_costmap lanelet_costmap multi_layered_costmap predicted_object_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt new file mode 100644 index 00000000..dd052e14 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(cached_lanelet_costmap) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_library(cached_lanelet_costmap SHARED + src/cached_lanelet_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp new file mode 100644 index 00000000..9bb7f072 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 Booars +// +// 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 CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_ +#define CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include + +#include + +namespace cached_lanelet_costmap +{ + +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; +using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters; +using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + +class CachedLaneletCostmap : public multi_layered_costmap::CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + explicit CachedLaneletCostmap(rclcpp::Node & node, const std::string & layer_namespace); + + static CostmapBase::SharedPtr create_costmap( + rclcpp::Node & node, const std::string & layer_namespace) + { + return std::make_shared(node, layer_namespace); + } + + bool is_ready() override; + bool is_occupied(const geometry_msgs::msg::PointStamped & point) override; + +private: + bool try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame); + void create_cached_costmap( + const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets); + void map_callback(const HADMapBin::SharedPtr msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Publisher::SharedPtr costmap_pub_; + + OccupancyGrid::SharedPtr cached_costmap_; + OccupancyGridParameters::SharedPtr cached_costmap_parameters_; + + double cached_costmap_origin_x_; + double cached_costmap_origin_y_; + bool is_map_ready_ = false; +}; +} // namespace cached_lanelet_costmap + +#endif // CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml new file mode 100644 index 00000000..83283207 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml @@ -0,0 +1,25 @@ + + + + cached_lanelet_costmap + 0.0.0 + The cached_lanelet_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_mapping_msgs + booars_utils + geometry_msgs + lanelet2_extension + multi_layered_costmap + rclcpp + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp new file mode 100644 index 00000000..8f206d03 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp @@ -0,0 +1,144 @@ +// Copyright 2024 Booars +// +// 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 "cached_lanelet_costmap/cached_lanelet_costmap.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include + +namespace cached_lanelet_costmap +{ +CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::string & layer_namespace) +: tf_buffer_(node.get_clock()), tf_listener_(tf_buffer_) +{ + std::string map_topic = node.declare_parameter(layer_namespace + ".map_topic", "~/input/map"); + map_sub_ = node.create_subscription( + map_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(), + std::bind(&CachedLaneletCostmap::map_callback, this, std::placeholders::_1)); + + std::string costmap_topic = + node.declare_parameter(layer_namespace + ".costmap_topic", "~/output/costmap"); + costmap_pub_ = node.create_publisher( + costmap_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local()); + + { + double cached_costmap_min_x = + node.declare_parameter(layer_namespace + ".cached_costmap.min_x", 0.0); + double cached_costmap_min_y = + node.declare_parameter(layer_namespace + ".cached_costmap.min_y", 0.0); + double cached_costmap_max_x = + node.declare_parameter(layer_namespace + ".cached_costmap.max_x", 0.0); + double cached_costmap_max_y = + node.declare_parameter(layer_namespace + ".cached_costmap.max_y", 0.0); + double cached_costmap_resolution = + node.declare_parameter(layer_namespace + ".cached_costmap.resolution", 0.0); + + cached_costmap_parameters_ = OccupancyGridParameters::create_parameters( + cached_costmap_max_x - cached_costmap_min_x, cached_costmap_max_y - cached_costmap_min_y, + cached_costmap_resolution); + cached_costmap_origin_x_ = (cached_costmap_min_x + cached_costmap_max_x) * 0.5; + cached_costmap_origin_y_ = (cached_costmap_min_y + cached_costmap_max_y) * 0.5; + + cached_costmap_ = + booars_utils::nav::occupancy_grid_utils::create_occupancy_grid(cached_costmap_parameters_); + + geometry_msgs::msg::Vector3 origin; + origin.x = cached_costmap_origin_x_; + origin.y = cached_costmap_origin_y_; + origin.z = 0.0; + booars_utils::nav::occupancy_grid_utils::update_origin( + cached_costmap_, cached_costmap_parameters_, origin); + } +} + +bool CachedLaneletCostmap::is_ready() +{ + return is_map_ready_; +} + +bool CachedLaneletCostmap::is_occupied(const geometry_msgs::msg::PointStamped & point) +{ + if (!is_ready()) return true; + + geometry_msgs::msg::Point transformed_point; + if (!try_transform_point(point, transformed_point, cached_costmap_->header.frame_id)) return true; + + tier4_autoware_utils::Point2d point2d( + transformed_point.x - cached_costmap_origin_x_, transformed_point.y - cached_costmap_origin_y_); + if ( + point2d[0] < -cached_costmap_parameters_->width_2() || + point2d[0] > cached_costmap_parameters_->width_2() || + point2d[1] < -cached_costmap_parameters_->height_2() || + point2d[1] > cached_costmap_parameters_->height_2()) { + return true; + } + int index = + booars_utils::nav::occupancy_grid_utils::point_to_index(cached_costmap_parameters_, point2d); + if (index < 0 || index >= cached_costmap_parameters_->grid_num()) return true; + return cached_costmap_->data[index] > 0; +} + +bool CachedLaneletCostmap::try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame) +{ + geometry_msgs::msg::PointStamped transformed_point_stamped; + try { + transformed_point_stamped = tf_buffer_.transform(point, target_frame); + } catch (tf2::TransformException & ex) { + return false; + } + + transformed_point = transformed_point_stamped.point; + return true; +} + +void CachedLaneletCostmap::create_cached_costmap( + const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets) +{ + cached_costmap_->header.frame_id = map_frame_id; + for (int i = 0; i < cached_costmap_parameters_->grid_num(); i++) { + tier4_autoware_utils::Point2d point2d = + booars_utils::nav::occupancy_grid_utils::index_to_point(cached_costmap_parameters_, i); + point2d[0] += cached_costmap_origin_x_; + point2d[1] += cached_costmap_origin_y_; + cached_costmap_->data[i] = 100; + for (const auto & road_lanelet : roads_lanelets) { + if (!lanelet::geometry::within(point2d, road_lanelet.polygon2d().basicPolygon())) continue; + cached_costmap_->data[i] = 0; + break; + } + } + costmap_pub_->publish(*cached_costmap_); + + is_map_ready_ = true; +} + +void CachedLaneletCostmap::map_callback(const HADMapBin::SharedPtr msg) +{ + lanelet::LaneletMapPtr map = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, map); + const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map); + const lanelet::ConstLanelets roads_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + create_cached_costmap(msg->header.frame_id, roads_lanelets); +} +} // namespace cached_lanelet_costmap From 8519d01033f08bc4007428a204bab32a215f1501 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 5 Sep 2024 01:03:05 +0900 Subject: [PATCH 30/48] implement costmap inflation Signed-off-by: Autumn60 --- .../cached_lanelet_costmap.hpp | 1 + .../src/cached_lanelet_costmap.cpp | 34 +++++++++++++++++++ 2 files changed, 35 insertions(+) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp index 9bb7f072..a31b1d89 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp @@ -68,6 +68,7 @@ class CachedLaneletCostmap : public multi_layered_costmap::CostmapBase double cached_costmap_origin_x_; double cached_costmap_origin_y_; + int inflation_radius_index_; bool is_map_ready_ = false; }; } // namespace cached_lanelet_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp index 8f206d03..5864c526 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp @@ -40,6 +40,7 @@ CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::strin costmap_pub_ = node.create_publisher( costmap_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local()); + // Declare cached costmap { double cached_costmap_min_x = node.declare_parameter(layer_namespace + ".cached_costmap.min_x", 0.0); @@ -68,6 +69,12 @@ CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::strin booars_utils::nav::occupancy_grid_utils::update_origin( cached_costmap_, cached_costmap_parameters_, origin); } + + // Declare other parameters + { + double inflation_radius = node.declare_parameter(layer_namespace + ".inflation_radius", 0.0); + inflation_radius_index_ = std::ceil(inflation_radius / cached_costmap_parameters_->resolution()); + } } bool CachedLaneletCostmap::is_ready() @@ -115,6 +122,9 @@ bool CachedLaneletCostmap::try_transform_point( void CachedLaneletCostmap::create_cached_costmap( const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets) { + std::vector unoccupied_indices; + + // Create costmap cached_costmap_->header.frame_id = map_frame_id; for (int i = 0; i < cached_costmap_parameters_->grid_num(); i++) { tier4_autoware_utils::Point2d point2d = @@ -125,9 +135,33 @@ void CachedLaneletCostmap::create_cached_costmap( for (const auto & road_lanelet : roads_lanelets) { if (!lanelet::geometry::within(point2d, road_lanelet.polygon2d().basicPolygon())) continue; cached_costmap_->data[i] = 0; + unoccupied_indices.push_back(i); break; } } + + // Inflate costmap + if (inflation_radius_index_ > 0) { + int inflation_radius_index_sqr = inflation_radius_index_ * inflation_radius_index_; + for (const auto & unoccupied_index : unoccupied_indices) + { + bool occupied = false; + for (int i = -inflation_radius_index_; i <= inflation_radius_index_; i++) { + for (int j = -inflation_radius_index_; j <= inflation_radius_index_; j++) { + if (i*i + j*j > inflation_radius_index_sqr) continue; + int index = unoccupied_index + i * cached_costmap_parameters_->grid_width() + j; + if (index < 0 || index >= cached_costmap_parameters_->grid_num()) continue; + if(cached_costmap_->data[index] != 100) continue; + cached_costmap_->data[unoccupied_index] = 50; + occupied = true; + break; + } + if(occupied) break; + } + } + } + + // Publish costmap costmap_pub_->publish(*cached_costmap_); is_map_ready_ = true; From 5dda6f6f54bda7b5f10e8e9b008bdcd80020de89 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 5 Sep 2024 01:03:16 +0900 Subject: [PATCH 31/48] adjust params for costmap inflation Signed-off-by: Autumn60 --- .../planning/costmap_generator.param.yaml | 17 +++++++++-------- .../config/costmap_generator.param.yaml | 1 + 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index 613dd9d7..9633f84e 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -1,10 +1,10 @@ /**: ros__parameters: - update_rate: 20.0 + update_rate: 20.0 # [Hz] map_frame_id: "map" costmap_center_frame_id: "base_link" - costmap_width: 50.0 - costmap_resolution: 0.2 + costmap_width: 50.0 # [m] + costmap_resolution: 0.2 # [m/cell] multi_layered_costmap: layers: - "cached_lanelet_layer" @@ -13,12 +13,13 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" + inflation_radius: 0.6 # [m] cached_costmap: - min_x: 89607.0 - max_x: 89687.0 - min_y: 43114.0 - max_y: 43194.0 - resolution: 0.1 + min_x: 89607.0 # [m] + max_x: 89687.0 # [m] + min_y: 43114.0 # [m] + max_y: 43194.0 # [m] + resolution: 0.1 # [m/cell] predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml index 3f461d36..2b89b1e2 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml @@ -14,6 +14,7 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" + inflation_radius: 0.5 cached_costmap: min_x: 89607.0 max_x: 89687.0 From 837cd05fa64d8d53870d108bf2d580f19d832af7 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 5 Sep 2024 20:05:02 +0900 Subject: [PATCH 32/48] feat: costmap inflation (#31) * implement costmap inflation Signed-off-by: Autumn60 * adjust params for costmap inflation Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .../planning/costmap_generator.param.yaml | 17 +++++----- .../config/costmap_generator.param.yaml | 1 + .../cached_lanelet_costmap.hpp | 1 + .../src/cached_lanelet_costmap.cpp | 34 +++++++++++++++++++ 4 files changed, 45 insertions(+), 8 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index 613dd9d7..9633f84e 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -1,10 +1,10 @@ /**: ros__parameters: - update_rate: 20.0 + update_rate: 20.0 # [Hz] map_frame_id: "map" costmap_center_frame_id: "base_link" - costmap_width: 50.0 - costmap_resolution: 0.2 + costmap_width: 50.0 # [m] + costmap_resolution: 0.2 # [m/cell] multi_layered_costmap: layers: - "cached_lanelet_layer" @@ -13,12 +13,13 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" + inflation_radius: 0.6 # [m] cached_costmap: - min_x: 89607.0 - max_x: 89687.0 - min_y: 43114.0 - max_y: 43194.0 - resolution: 0.1 + min_x: 89607.0 # [m] + max_x: 89687.0 # [m] + min_y: 43114.0 # [m] + max_y: 43194.0 # [m] + resolution: 0.1 # [m/cell] predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml index 3f461d36..2b89b1e2 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml @@ -14,6 +14,7 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" + inflation_radius: 0.5 cached_costmap: min_x: 89607.0 max_x: 89687.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp index 9bb7f072..a31b1d89 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp @@ -68,6 +68,7 @@ class CachedLaneletCostmap : public multi_layered_costmap::CostmapBase double cached_costmap_origin_x_; double cached_costmap_origin_y_; + int inflation_radius_index_; bool is_map_ready_ = false; }; } // namespace cached_lanelet_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp index 8f206d03..5864c526 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp @@ -40,6 +40,7 @@ CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::strin costmap_pub_ = node.create_publisher( costmap_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local()); + // Declare cached costmap { double cached_costmap_min_x = node.declare_parameter(layer_namespace + ".cached_costmap.min_x", 0.0); @@ -68,6 +69,12 @@ CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::strin booars_utils::nav::occupancy_grid_utils::update_origin( cached_costmap_, cached_costmap_parameters_, origin); } + + // Declare other parameters + { + double inflation_radius = node.declare_parameter(layer_namespace + ".inflation_radius", 0.0); + inflation_radius_index_ = std::ceil(inflation_radius / cached_costmap_parameters_->resolution()); + } } bool CachedLaneletCostmap::is_ready() @@ -115,6 +122,9 @@ bool CachedLaneletCostmap::try_transform_point( void CachedLaneletCostmap::create_cached_costmap( const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets) { + std::vector unoccupied_indices; + + // Create costmap cached_costmap_->header.frame_id = map_frame_id; for (int i = 0; i < cached_costmap_parameters_->grid_num(); i++) { tier4_autoware_utils::Point2d point2d = @@ -125,9 +135,33 @@ void CachedLaneletCostmap::create_cached_costmap( for (const auto & road_lanelet : roads_lanelets) { if (!lanelet::geometry::within(point2d, road_lanelet.polygon2d().basicPolygon())) continue; cached_costmap_->data[i] = 0; + unoccupied_indices.push_back(i); break; } } + + // Inflate costmap + if (inflation_radius_index_ > 0) { + int inflation_radius_index_sqr = inflation_radius_index_ * inflation_radius_index_; + for (const auto & unoccupied_index : unoccupied_indices) + { + bool occupied = false; + for (int i = -inflation_radius_index_; i <= inflation_radius_index_; i++) { + for (int j = -inflation_radius_index_; j <= inflation_radius_index_; j++) { + if (i*i + j*j > inflation_radius_index_sqr) continue; + int index = unoccupied_index + i * cached_costmap_parameters_->grid_width() + j; + if (index < 0 || index >= cached_costmap_parameters_->grid_num()) continue; + if(cached_costmap_->data[index] != 100) continue; + cached_costmap_->data[unoccupied_index] = 50; + occupied = true; + break; + } + if(occupied) break; + } + } + } + + // Publish costmap costmap_pub_->publish(*cached_costmap_); is_map_ready_ = true; From f24c6758b2f4aae248aa8e768db1e8b6c6a779ac Mon Sep 17 00:00:00 2001 From: tamago117 Date: Thu, 5 Sep 2024 21:15:55 +0900 Subject: [PATCH 33/48] adjust param --- .../planning/costmap_generator.param.yaml | 4 +- .../planning/mppi_controller.param.yaml | 26 + .../booars_launch/launch/booars.launch.xml | 2 +- .../launch/components/control.launch.xml | 2 +- .../launch/components/planning.launch.xml | 2 +- .../booars_launch/map/lanelet2_map_opt.osm | 5710 +++++++++++++++++ .../config/mppi_controller.param.yaml | 2 + .../config/autoware.rviz | 48 +- 8 files changed, 5788 insertions(+), 8 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index 9633f84e..ea3e4422 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -13,7 +13,7 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" - inflation_radius: 0.6 # [m] + inflation_radius: 1.8 # [m] cached_costmap: min_x: 89607.0 # [m] max_x: 89687.0 # [m] @@ -23,4 +23,4 @@ predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" - distance_threshold: 1.2 + distance_threshold: 1.3 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml new file mode 100644 index 00000000..ca77d864 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + # mppi + horizon : 25 + num_samples : 3000 + u_min : [-2.0, -0.35] # accel(m/s2), steer angle(rad) + u_max : [2.0, 0.35] + sigmas : [0.5, 0.25] # sample range + lambda : 1.0 + auto_lambda : false + # reference path + DL : 0.1 + lookahead_distance : 0.1 + reference_path_interval : 0.8 + # cost weights + Qc : 20.0 + Ql : 1.0 + Qv : 2.0 + Qo : 10000.0 + Qin : 0.01 + Qdin : 0.5 + # model param + delta_t : 0.1 + vehicle_L : 1.0 + V_MAX : 8.0 + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml index 642e91c8..a5acfb29 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index 6458d746..bddbb773 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index 24425f1b..d1c3d1c8 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -114,7 +114,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm new file mode 100644 index 00000000..28d60946 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm @@ -0,0 +1,5710 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml index 8f0d35a0..d4b1c4ce 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # booares_launchの方参照されたし + # mppi horizon : 25 num_samples : 3000 diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index 9e75c143..ba3f9b7b 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -8,8 +8,9 @@ Panels: - /Planning1/ScenarioPlanning1/ScenarioTrajectory1 - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1 - /Planning1/Costmap1/Map1 - - /Planning1/MPPI1 - /Planning1/MPPI1/Trajectory1 + - /Planning1/MPPI1/ReferenceTrajectory1 + - /Planning1/MPPI1/ReferenceTrajectory1/View Path1 Splitter Ratio: 0.557669460773468 Tree Height: 158 - Class: rviz_common/Selection @@ -1993,7 +1994,48 @@ Visualization Manager: Color: 0; 0; 0 Constant Color: false Value: true - Width: 0.5 + Width: 0.6000000238418579 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ReferenceTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/path + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 170; 0; 0 + Constant Color: true + Value: true + Width: 2 View Point: Alpha: 1 Color: 0; 60; 255 @@ -2219,7 +2261,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 7.122405529022217 + Scale: 9.442424774169922 Target Frame: viewer Value: TopDownOrtho (rviz_default_plugins) X: 2.5461606979370117 From 6b00422805bdd80fbaccdaf47c8cab18607817a2 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Thu, 5 Sep 2024 21:19:40 +0900 Subject: [PATCH 34/48] fix typo --- .../mppi_controller/config/mppi_controller.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml index d4b1c4ce..3a03a3e8 100644 --- a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - # booares_launchの方参照されたし + # booars_launchの方参照されたし # mppi horizon : 25 From a33399db532e53b4acd7e218ee580c9ae4687fe5 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Fri, 6 Sep 2024 08:18:11 +0900 Subject: [PATCH 35/48] chore(control): parameterization control mode (#32) * create use_stanley param Signed-off-by: Autumn60 * Update aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml Co-authored-by: sitahara <33857284+sitahara@users.noreply.github.com> --------- Signed-off-by: Autumn60 Co-authored-by: sitahara <33857284+sitahara@users.noreply.github.com> --- .../launch/components/control.launch.xml | 50 +++++++++++-------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index 6458d746..1b944d31 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -1,30 +1,36 @@ + + + - - - - - - - + + + + + + + + + + + + + + - - - - + + + + + + + + + - - From 2fba0e84a529dd8fcf92372d4cda40c4c1d8286a Mon Sep 17 00:00:00 2001 From: Bey9434 <134945706+Bey9434@users.noreply.github.com> Date: Fri, 6 Sep 2024 18:30:30 +0900 Subject: [PATCH 36/48] Fixed typos in functions (#35) --- .../include/booars_utils/nav/occupancy_grid_parameters.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp index 8c32ac5c..3fb81ad7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp @@ -53,7 +53,7 @@ class OccupancyGridParameters double resolution_inv() const { return resolution_inv_; } int grid_width_2() const { return grid_width_2_; } int grid_width() const { return grid_width_; } - int gird_height_2() const { return grid_height_2_; } + int grid_height_2() const { return grid_height_2_; } int grid_height() const { return grid_height_; } int grid_num() const { return grid_num_; } From 0730ecd0c1f6b254c8c108a7cbbcbab91364ee21 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Mon, 9 Sep 2024 16:21:29 +0900 Subject: [PATCH 37/48] Update costmap_generator.cpp (#37) --- .../booars_costmap_generator/src/costmap_generator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp index 60b34007..a4d182b3 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp @@ -80,14 +80,14 @@ void CostmapGenerator::update() // Fill the costmap data { + PointStamped global_point; + global_point.header.frame_id = map_frame_; + global_point.header.stamp = costmap_time; + global_point.point.z = 0.0; for (int i = 0; i < costmap_parameters_->grid_num(); i++) { Point2d local_point = index_to_point_table_[i]; - PointStamped global_point; - global_point.header.frame_id = map_frame_; - global_point.header.stamp = costmap_time; global_point.point.x = local_point[0] + costmap_center_position.x; global_point.point.y = local_point[1] + costmap_center_position.y; - global_point.point.z = 0.0; costmap_->data[i] = multi_layered_costmap_->is_occupied(global_point) ? 100 : 0; } } From 1ef8db4940752a149f24c717ddc643edfa6cb0c1 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Wed, 18 Sep 2024 22:55:01 +0900 Subject: [PATCH 38/48] add "gird" to flagWords in .cspell.json (#36) Signed-off-by: Autumn60 --- .cspell.json | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.cspell.json b/.cspell.json index 7978a84c..9c49459b 100644 --- a/.cspell.json +++ b/.cspell.json @@ -46,6 +46,9 @@ "ignoreRegExpList": ["@author .*$", "[\\@]tparam", "\\author .*$", "Author(s)?( )?: .*$", "TODO\\((.*?)\\)"] } ], + "flagWords": [ + "gird" + ], "words": [ "ackermann", "adapi", From eec2295fa6e9924c6bbe46b631fc521a56392944 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 19 Sep 2024 17:26:39 +0900 Subject: [PATCH 39/48] add "linestring", "linestrings", "astr", "rrtstar" to spell dict (#38) Signed-off-by: Autumn60 --- .cspell.json | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.cspell.json b/.cspell.json index 9c49459b..2c51a351 100644 --- a/.cspell.json +++ b/.cspell.json @@ -53,6 +53,7 @@ "ackermann", "adapi", "aichallenge", + "astar", "autocompute", "automotiveaichallenge", "autoware", @@ -79,6 +80,8 @@ "libgazebo", "libgl", "linalg", + "linestring", + "linestrings", "lowpass", "mapfile", "mathcal", @@ -94,6 +97,7 @@ "rois", "rosdep", "rosdistro", + "rrtstar", "rviz", "schematypens", "sideshift", From 70db0dc91d4ddae1f2bff573ada23110aa83d6be Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 2 Oct 2024 23:10:53 +0900 Subject: [PATCH 40/48] add words to .cspell.json Signed-off-by: Autumn60 --- .cspell.json | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.cspell.json b/.cspell.json index 2c51a351..66c0f12b 100644 --- a/.cspell.json +++ b/.cspell.json @@ -50,15 +50,18 @@ "gird" ], "words": [ + "accelmap", "ackermann", "adapi", "aichallenge", "astar", "autocompute", + "autodetermine", "automotiveaichallenge", "autoware", "awsim", "booars", + "brakemap", "buildtool", "colcon", "costmap", @@ -66,6 +69,7 @@ "cyclonedds", "dallara", "dcmake", + "decel", "distro", "downsample", "freespace", From 36809d616ba8edeab0051abdd8ea1724af039c84 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 3 Oct 2024 01:52:23 +0900 Subject: [PATCH 41/48] add parameter files Signed-off-by: Autumn60 --- .../booars_launch/CMakeLists.txt | 2 +- .../config/behavior_path_planner.param.yaml | 69 + .../config/behavior_path_planner_tree.xml | 74 + .../config/map/lanelet2_map_loader.param.yaml | 4 + .../planning/costmap_generator.param.yaml | 2 +- .../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 + .../vehicle_velocity_converter.param.yaml | 6 + .../raw_vehicle_cmd_converter.param.yaml | 18 + .../booars_launch/data/accel_map.csv | 10 + .../booars_launch/data/brake_map.csv | 9 + .../booars_launch/data/steer_map.csv | 14 + .../booars_launch/map/lanelet2_map.osm | 4683 +++++------------ 20 files changed, 1914 insertions(+), 3336 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner_tree.xml create mode 100755 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/map/lanelet2_map_loader.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/scenario_planning/common/nearest_search.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_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/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/vehicle_velocity_converter.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/vehicle/raw_vehicle_cmd_converter.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/data/accel_map.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/data/brake_map.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/data/steer_map.csv diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt index 6440f8e3..1c6a984b 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/CMakeLists.txt @@ -3,4 +3,4 @@ project(booars_launch) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_package(INSTALL_TO_SHARE launch map config) +ament_auto_package(INSTALL_TO_SHARE launch map config data) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner.param.yaml new file mode 100644 index 00000000..0337933c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner.param.yaml @@ -0,0 +1,69 @@ +/**: + ros__parameters: + verbose: false + + planning_hz: 10.0 + backward_path_length: 5.0 + forward_path_length: 300.0 + backward_length_buffer_for_end_of_pull_over: 5.0 + backward_length_buffer_for_end_of_pull_out: 5.0 + minimum_pull_over_length: 16.0 + + refine_goal_search_radius_range: 7.5 + + # parameters for turn signal decider + turn_signal_intersection_search_distance: 30.0 + turn_signal_intersection_angle_threshold_deg: 15.0 + turn_signal_minimum_search_distance: 10.0 + turn_signal_search_time: 3.0 + turn_signal_shift_length_threshold: 0.3 + turn_signal_on_swerving: true + + path_interval: 1.0 + + visualize_maximum_drivable_area: true + + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.0 + + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + + # USE ONLY WHEN THE OPTION COMPILE_WITH_OLD_ARCHITECTURE IS SET TO FALSE. + # https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/CMakeLists.txt + # NOTE: The smaller the priority number is, the higher the module priority is. + lane_change: + enable_module: true + enable_simultaneous_execution: false + priority: 4 + max_module_size: 1 + + pull_out: + enable_module: true + enable_simultaneous_execution: false + priority: 0 + max_module_size: 1 + + side_shift: + enable_module: true + enable_simultaneous_execution: false + priority: 2 + max_module_size: 1 + + pull_over: + enable_module: true + enable_simultaneous_execution: false + priority: 1 + max_module_size: 1 + + avoidance: + enable_module: true + enable_simultaneous_execution: false + priority: 3 + max_module_size: 1 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner_tree.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner_tree.xml new file mode 100644 index 00000000..23a61402 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/behavior_path_planner_tree.xml @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + desc + + + desc + + + + + + desc + + + desc + + + + desc + + + desc + + + + + + + + desc + + + + + desc + + + desc + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/map/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/map/lanelet2_map_loader.param.yaml new file mode 100755 index 00000000..61c41ee8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/map/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_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index ea3e4422..f670c1e7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -8,7 +8,7 @@ multi_layered_costmap: layers: - "cached_lanelet_layer" - - "predicted_object_layer" + # - "predicted_object_layer" cached_lanelet_layer: type: "cached_lanelet" map_topic: "/map/vector_map" diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/common/nearest_search.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_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/booars_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/booars_launch/config/sensing/vehicle_velocity_converter.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/vehicle_velocity_converter.param.yaml new file mode 100644 index 00000000..649296ff --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/sensing/vehicle_velocity_converter.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + speed_scale_factor: 1.0 # [.] + velocity_stddev_xx: 0.2 # [m/s] + angular_velocity_stddev_zz: 0.1 # [rad/s] + frame_id: base_link diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/vehicle/raw_vehicle_cmd_converter.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/vehicle/raw_vehicle_cmd_converter.param.yaml new file mode 100644 index 00000000..3e98b794 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/vehicle/raw_vehicle_cmd_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_submit/booars_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/data/accel_map.csv new file mode 100644 index 00000000..95879ae7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/data/accel_map.csv @@ -0,0 +1,10 @@ +default, 0.0 ,2.0 ,4.0 ,6.0 ,8.0 ,10.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.01 ,0.01 ,0.01 ,0.01 ,0.01 ,0.01 + 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 + 0.8, 0.7 ,0.7 ,0.7 ,0.7 ,0.7 ,0.7 + 1.0, 1.0 ,1.0 ,1.0 ,1.0 ,1.0 ,1.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/data/brake_map.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/data/brake_map.csv new file mode 100644 index 00000000..3a059f61 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/data/brake_map.csv @@ -0,0 +1,9 @@ + default, 0.0, 2.0, 4.0, 6.0, 8.0, 10.0 + 0.0, -0.3, -0.3, -0.3, -0.3, -0.3, -0.3 + 0.1, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5 + 0.2, -0.7, -0.7, -0.7, -0.7, -0.7, -0.7 + 0.3, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9 + 0.4, -1.1, -1.1, -1.1, -1.1, -1.1, -1.1 + 0.5, -1.3, -1.3, -1.3, -1.3, -1.3, -1.3 + 0.6, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5 + 1.0, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/data/steer_map.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/data/steer_map.csv new file mode 100644 index 00000000..3da0e0cd --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_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_submit/booars_launch/map/lanelet2_map.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm index 0c9222a4..49147919 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm @@ -1,5126 +1,3288 @@ - - - + + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - + + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - - - - - - - - - - - - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - - - - - - - + - - + - - - - - - - - + - - - - - - - - - - - - - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - - - - - - - + - - + - - + - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - - - + + + - - - - - - - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - - - - - - - + - - + - - - - - - - - + - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - - - - - - - - - - - - + + + - - + - - + - - - + + - - - + + - - - + + - - - + + - - - - - - - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - - - - - - - - - - - - + + - - - + + - - - - - - - - - - - - - - - - + + + - - - - + + + - - + - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - - - + + + - - + - - - - - - - - + - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - - - - - + - - - - - - - - + - - - - + + + - - - - + + + - - + - - - - + + + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - + - - + - - + - - - - - - - - + - - - - + + + - - - - - - - - + - - - - - - - - - - - - - - - - - - - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - + + + - - + + + + + + + + + + + + + + + + + + + + + + @@ -5133,7 +3295,6 @@ - @@ -5149,7 +3310,234 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -5183,7 +3571,6 @@ - @@ -5198,16 +3585,11 @@ - - - - - @@ -5220,30 +3602,20 @@ - - - - - - - - - - @@ -5251,177 +3623,11 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - + @@ -5437,15 +3643,10 @@ - - - - - @@ -5503,35 +3704,173 @@ - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - @@ -5550,17 +3889,11 @@ - - - - - - @@ -5589,6 +3922,12 @@ + + + + + + @@ -5604,16 +3943,11 @@ - - - - - @@ -5625,10 +3959,8 @@ - - @@ -5665,360 +3997,43 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + - + + + + + + + + + + + + + + + + + + + + + From 527ce612993ab45233b74009c08a3abfdc884750 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 3 Oct 2024 01:52:41 +0900 Subject: [PATCH 42/48] update booars_launch for sync-upstream Signed-off-by: Autumn60 --- .../launch/aichallenge_submit.launch.xml | 6 +- .../booars_launch/launch/booars.launch.xml | 48 ++++++++------ .../launch/components/control.launch.xml | 5 ++ .../launch/components/localization.launch.xml | 62 +++++++++++-------- .../launch/components/map.launch.xml | 5 +- .../launch/components/perception.launch.xml | 11 +++- .../launch/components/planning.launch.xml | 22 +++---- .../launch/components/sensing.launch.xml | 25 +++++--- .../launch/components/vehicle.launch.xml | 23 ++++++- 9 files changed, 130 insertions(+), 77 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml index a625ec50..da639962 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml @@ -5,16 +5,16 @@ - + - + - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml index a5acfb29..91d5d236 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/booars.launch.xml @@ -1,46 +1,54 @@ - - - - - - - + + + + + + - + + + + + + - - - + + + - - + + - + - + - + - + - + - + + + @@ -51,4 +59,4 @@ - + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index 529b944a..350d33c5 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -1,11 +1,15 @@ + + + + @@ -13,6 +17,7 @@ + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml index ab77ad89..c8514f57 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/localization.launch.xml @@ -1,42 +1,52 @@ + + + - + - - - - + + + + - + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - - - - + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml index e5d3e09d..5b9704ca 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/map.launch.xml @@ -15,7 +15,7 @@ - + @@ -24,7 +24,7 @@ - + @@ -36,6 +36,5 @@ - \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml index a16e4d85..7e3f2371 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/perception.launch.xml @@ -2,9 +2,16 @@ - + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index d1c3d1c8..c1deb358 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -1,5 +1,6 @@ + @@ -13,7 +14,7 @@ - + @@ -73,19 +74,19 @@ - + - - - - - - - - + + + + + + + + @@ -121,7 +122,6 @@ - diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml index 403f1b21..74da9d28 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/sensing.launch.xml @@ -2,22 +2,27 @@ - - + + - - - + + + + - - + + - - - + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml index 97d01413..31508ca9 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/vehicle.launch.xml @@ -1,6 +1,25 @@ - - + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From ac046e7bac716e21d1dd7d10981a7de37bc5504d Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 3 Oct 2024 23:09:06 +0900 Subject: [PATCH 43/48] feat(mission_planning): add pit-in control (rebased) (#52) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * pit in path * chore: Update goal_pose_setter configuration to enable pit and adjust threshold * ignore result json * add pit in speed control * add spell * fixed bug * add mission planner param * mission planner param 調整 * pitin ok * param fileをbooars_launchに移行 * fix typo * parameter tuning * parameter update * add .gitignore for .vscode Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 Co-authored-by: hrjp Co-authored-by: tamago117 --- .cspell.json | 3 +- .gitignore | 1 + aichallenge/.gitignore | 2 + aichallenge/workspace/.gitignore | 3 +- .../launch/aichallenge_submit.launch.xml | 2 +- .../planning/costmap_generator.param.yaml | 6 +- .../config/planning/goal_pose.param.yaml | 32 + .../planning/mission_planner.param.yaml | 13 + .../planning/mppi_controller.param.yaml | 20 +- .../launch/components/control.launch.xml | 6 +- .../launch/components/planning.launch.xml | 10 +- .../booars_launch/map/lanelet2_map_opt.osm | 2512 +++----- .../map/lanelet2_map_opt_pre.osm | 5710 +++++++++++++++++ .../config/default_goal_pose.param.yaml | 11 + .../goal_pose_setter/package.xml | 1 + .../src/goal_pose_setter_node.cpp | 89 +- .../src/goal_pose_setter_node.hpp | 19 + .../path_to_trajectory/path_to_trajectory.hpp | 4 + .../src/path_to_trajectory.cpp | 26 + .../src/simple_pure_pursuit.cpp | 33 +- .../config/autoware.rviz | 37 +- 21 files changed, 6815 insertions(+), 1725 deletions(-) create mode 100644 .gitignore create mode 100644 aichallenge/.gitignore create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm diff --git a/.cspell.json b/.cspell.json index 66c0f12b..df7e9be3 100644 --- a/.cspell.json +++ b/.cspell.json @@ -137,6 +137,7 @@ "tempature", "rsample", "coeffs", - "softplus" + "softplus", + "mpss" ] } diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..600d2d33 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode \ No newline at end of file diff --git a/aichallenge/.gitignore b/aichallenge/.gitignore new file mode 100644 index 00000000..71c36e20 --- /dev/null +++ b/aichallenge/.gitignore @@ -0,0 +1,2 @@ +capture +result-details.json \ No newline at end of file diff --git a/aichallenge/workspace/.gitignore b/aichallenge/workspace/.gitignore index ab2524be..fca9e03b 100644 --- a/aichallenge/workspace/.gitignore +++ b/aichallenge/workspace/.gitignore @@ -3,4 +3,5 @@ /log .vscode -*.code-workspace \ No newline at end of file +*.code-workspace +result-details.json diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml index da639962..ec2952a0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml @@ -22,6 +22,6 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index f670c1e7..283e0a7f 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -4,7 +4,7 @@ map_frame_id: "map" costmap_center_frame_id: "base_link" costmap_width: 50.0 # [m] - costmap_resolution: 0.2 # [m/cell] + costmap_resolution: 0.1 # [m/cell] multi_layered_costmap: layers: - "cached_lanelet_layer" @@ -13,7 +13,7 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" - inflation_radius: 1.8 # [m] + inflation_radius: 1.4 # [m] cached_costmap: min_x: 89607.0 # [m] max_x: 89687.0 # [m] @@ -23,4 +23,4 @@ predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" - distance_threshold: 1.3 + distance_threshold: 1.2 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml new file mode 100644 index 00000000..7f21479c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/goal_pose.param.yaml @@ -0,0 +1,32 @@ +/**: + ros__parameters: + goal.position.x: 89653.7 + goal.position.y: 43122.5 + goal.position.z: 0.0 + goal.orientation.x: 0.0 + goal.orientation.y: 0.0 + goal.orientation.z: -0.971732 + goal.orientation.w: 0.236088 + + half_goal.position.x: 89657.0 + half_goal.position.y: 43175.0 + half_goal.position.z: -28.0 + half_goal.orientation.x: 0.0 + half_goal.orientation.y: 0.0 + half_goal.orientation.z: -0.9 + half_goal.orientation.w: 0.25 + + pit_goal.position.x: 89626.3671875 + pit_goal.position.y: 43134.921875 + pit_goal.position.z: 42.10000228881836 + pit_goal.orientation.x: 0.0 + pit_goal.orientation.y: 0.0 + pit_goal.orientation.z: -0.8788172006607056 + pit_goal.orientation.w: -0.47715866565704346 + # ゴールまでの距離がこの値以下になると次のゴールを配信する + goal_range: 10.0 + # ピットインを有効化するか + enable_pit: true + # このしきい値以下になるとピットイン + # pit_in_threshold: 100 + pit_in_threshold: 700 \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml new file mode 100644 index 00000000..a0aa4f74 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mission_planner.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + map_frame: map + arrival_check_angle_deg: 180.0 + arrival_check_distance: 30.0 + arrival_check_duration: 1.0 + goal_angle_threshold_deg: 90.0 + enable_correct_goal_pose: false + reroute_time_threshold: 10.0 + minimum_reroute_length: 30.0 + consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. + check_footprint_inside_lanes: false + allow_reroute_in_autonomous_mode: true \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml index ca77d864..35ba01cc 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml @@ -2,10 +2,10 @@ ros__parameters: # mppi horizon : 25 - num_samples : 3000 - u_min : [-2.0, -0.35] # accel(m/s2), steer angle(rad) - u_max : [2.0, 0.35] - sigmas : [0.5, 0.25] # sample range + num_samples : 4000 + u_min : [-4.0, -0.35] # accel(m/s2), steer angle(rad) + u_max : [3.0, 0.35] + sigmas : [2.0, 0.35] # sample range lambda : 1.0 auto_lambda : false # reference path @@ -13,14 +13,14 @@ lookahead_distance : 0.1 reference_path_interval : 0.8 # cost weights - Qc : 20.0 + Qc : 10.0 Ql : 1.0 - Qv : 2.0 - Qo : 10000.0 + Qv : 4.0 + Qo : 1000.0 Qin : 0.01 - Qdin : 0.5 + Qdin : 0.0 # model param delta_t : 0.1 - vehicle_L : 1.0 - V_MAX : 8.0 + vehicle_L : 1.08 + V_MAX : 8.33 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index 350d33c5..d5d98a9c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -12,11 +12,11 @@ - + - - + + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index c1deb358..a1f3cb0c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -14,7 +14,7 @@ - + @@ -98,6 +98,14 @@ + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm index 28d60946..3754ea01 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt.osm @@ -1,4850 +1,4042 @@ - - - + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - + - - + - - + - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - + - - - - + + + - - + - + - - + - - + - + - - - - + + + - - + - - + - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + @@ -5693,7 +4885,7 @@ - + @@ -5703,7 +4895,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm new file mode 100644 index 00000000..28d60946 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map_opt_pre.osm @@ -0,0 +1,5710 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/config/default_goal_pose.param.yaml b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/config/default_goal_pose.param.yaml index 32dae394..87732d8f 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/config/default_goal_pose.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/config/default_goal_pose.param.yaml @@ -16,4 +16,15 @@ half_goal.orientation.z: -0.9 half_goal.orientation.w: 0.25 + pit_goal.position.x: 89626.3671875 + pit_goal.position.y: 43134.921875 + pit_goal.position.z: 42.10000228881836 + pit_goal.orientation.x: 0.0 + pit_goal.orientation.y: 0.0 + pit_goal.orientation.z: -0.8788172006607056 + pit_goal.orientation.w: -0.47715866565704346 + goal_range: 10.0 + + enable_pit: true + pit_in_threshold: 20 diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml index cca3510f..b508fbea 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/package.xml @@ -15,6 +15,7 @@ std_srvs nav2_msgs tier4_autoware_utils + std_msgs ament_cmake 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 bc2a50bf..c56d90f1 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 @@ -12,6 +12,33 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") odometry_subscriber_ = this->create_subscription( "/localization/kinematic_state", 1, std::bind(&GoalPosePublisher::odometry_callback, this, std::placeholders::_1)); + + pit_position_subscriber_ = this->create_subscription( + "/aichallenge/pitstop/area", 1, + [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) { + pit_position_.position.x = msg->data[0]; + pit_position_.position.y = msg->data[1]; + pit_position_.position.z = msg->data[2]; + pit_position_.orientation.x = msg->data[3]; + pit_position_.orientation.y = msg->data[4]; + pit_position_.orientation.z = msg->data[5]; + pit_position_.orientation.w = msg->data[6]; + }); + + pit_condition_subscriber_ = this->create_subscription( + "/aichallenge/pitstop/condition", 1, + [this](const std_msgs::msg::Int32::SharedPtr msg) { + pit_condition_ = msg->data; + }); + + pit_stop_time_subscriber_ = this->create_subscription( + "/aichallenge/pitstop/status", 1, + [this](const std_msgs::msg::Float32::SharedPtr msg) { + pit_stop_time_ = msg->data; + }); + + + timer_ = this->create_wall_timer( std::chrono::milliseconds(300), std::bind(&GoalPosePublisher::on_timer, this)); @@ -32,8 +59,19 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") this->declare_parameter("half_goal.orientation.z", -0.9); this->declare_parameter("half_goal.orientation.w", 0.25); + this->declare_parameter("pit_goal.position.x", 89626.3671875); + this->declare_parameter("pit_goal.position.y", 43134.921875); + this->declare_parameter("pit_goal.position.z", 42.10000228881836); + this->declare_parameter("pit_goal.orientation.x", 0.0); + this->declare_parameter("pit_goal.orientation.y", 0.0); + this->declare_parameter("pit_goal.orientation.z", -0.8788172006607056); + this->declare_parameter("pit_goal.orientation.w", -0.47715866565704346); + this->declare_parameter("goal_range", 10.0); + this->declare_parameter("enable_pit", true); + this->declare_parameter("pit_in_threshold", 1000); + goal_position_.position.x = this->get_parameter("goal.position.x").as_double(); goal_position_.position.y = this->get_parameter("goal.position.y").as_double(); goal_position_.position.z = this->get_parameter("goal.position.z").as_double(); @@ -51,7 +89,19 @@ GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher") half_goal_position_.orientation.z = this->get_parameter("half_goal.orientation.z").as_double(); half_goal_position_.orientation.w = this->get_parameter("half_goal.orientation.w").as_double(); + pit_position_.position.x = this->get_parameter("pit_goal.position.x").as_double(); + pit_position_.position.y = this->get_parameter("pit_goal.position.y").as_double(); + pit_position_.position.z = this->get_parameter("pit_goal.position.z").as_double(); + pit_position_.orientation.x = this->get_parameter("pit_goal.orientation.x").as_double(); + pit_position_.orientation.y = this->get_parameter("pit_goal.orientation.y").as_double(); + pit_position_.orientation.z = this->get_parameter("pit_goal.orientation.z").as_double(); + pit_position_.orientation.w = this->get_parameter("pit_goal.orientation.w").as_double(); + + goal_range_ = this->get_parameter("goal_range").as_double(); + + enable_pit_ = this->get_parameter("enable_pit").as_bool(); + pit_in_threshold_ = this->get_parameter("pit_in_threshold").as_int(); } void GoalPosePublisher::on_timer() @@ -107,22 +157,47 @@ void GoalPosePublisher::odometry_callback(const nav_msgs::msg::Odometry::SharedP { if (!is_started_) return; - + //RCLCPP_INFO(this->get_logger(), "%lf",pit_position_.position.x ); // Publish half goal pose for loop - if(half_goal_pose_published_ == false && - tier4_autoware_utils::calcDistance2d(msg->pose.pose, goal_position_) < goal_range_) + if(half_goal_pose_published_ == true && + tier4_autoware_utils::calcDistance2d(msg->pose.pose, goal_position_) < goal_range_ + && pit_in_flag_ == false) { auto goal_pose = std::make_shared(); goal_pose->header.stamp = this->get_clock()->now(); goal_pose->header.frame_id = "map"; - goal_pose->pose = half_goal_position_; + if(pit_condition_ > pit_in_threshold_ && enable_pit_ == true){ + goal_pose->pose = pit_position_; + pit_in_flag_ = true; + } + else{ + goal_pose->pose = half_goal_position_; + half_goal_pose_published_ = false; + } + + goal_publisher_->publish(*goal_pose); + RCLCPP_INFO(this->get_logger(), "Publishing half goal pose for loop"); + + } + if(pit_in_flag_ == true && + tier4_autoware_utils::calcDistance2d(msg->pose.pose, pit_position_) < goal_range_ + && pit_stop_time_ > 3.1) + { + auto goal_pose = std::make_shared(); + goal_pose->header.stamp = this->get_clock()->now(); + goal_pose->header.frame_id = "map"; + goal_pose->pose = half_goal_position_; + half_goal_pose_published_ = false; + pit_in_flag_ = false; + goal_publisher_->publish(*goal_pose); RCLCPP_INFO(this->get_logger(), "Publishing half goal pose for loop"); - half_goal_pose_published_ = true; + } + // Publish goal pose for loop - if (half_goal_pose_published_ == true && + if (half_goal_pose_published_ == false && tier4_autoware_utils::calcDistance2d(msg->pose.pose, half_goal_position_) < goal_range_) { auto goal_pose = std::make_shared(); @@ -132,7 +207,7 @@ void GoalPosePublisher::odometry_callback(const nav_msgs::msg::Odometry::SharedP goal_publisher_->publish(*goal_pose); RCLCPP_INFO(this->get_logger(), "Publishing goal pose for loop"); - half_goal_pose_published_ = false; + half_goal_pose_published_ = true; } } diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp index 0f4b3d20..a712d9e8 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp @@ -17,6 +17,10 @@ #include +#include +#include +#include + #include #include #include @@ -24,6 +28,8 @@ #include #include + + class GoalPosePublisher : public rclcpp::Node { public: @@ -38,6 +44,11 @@ class GoalPosePublisher : public rclcpp::Node rclcpp::Publisher::SharedPtr goal_publisher_; rclcpp::Subscription::SharedPtr route_state_subscriber_; rclcpp::Subscription::SharedPtr odometry_subscriber_; + + rclcpp::Subscription::SharedPtr pit_position_subscriber_; + rclcpp::Subscription::SharedPtr pit_condition_subscriber_; + rclcpp::Subscription::SharedPtr pit_stop_time_subscriber_; + rclcpp::TimerBase::SharedPtr timer_; bool stop_initializing_pose_ = false; bool stop_streaming_goal_pose_ = false; @@ -47,6 +58,14 @@ class GoalPosePublisher : public rclcpp::Node float goal_range_; geometry_msgs::msg::Pose goal_position_; geometry_msgs::msg::Pose half_goal_position_; + + geometry_msgs::msg::Pose pit_position_; + int pit_condition_; + float pit_stop_time_; + bool pit_in_flag_ = false; + + bool enable_pit_ = true; + int pit_in_threshold_ = 1000; }; #endif // GOAL_POSE_SETTER_NODE_ diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp index c24836f8..600fd08d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp @@ -32,6 +32,10 @@ class PathToTrajectory : public rclcpp::Node { void callback(const PathWithLaneId::SharedPtr msg); rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; + double deceleration_; + double stop_offset_; + double max_speed_; + double traj_width_; }; #endif // PATH_TO_TRAJECTORY__PATH_TO_TRAJECTORY_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp index 5e078e56..604d9a53 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/path_to_trajectory.cpp @@ -20,6 +20,16 @@ PathToTrajectory::PathToTrajectory() : Node("path_to_trajectory_node") { pub_ = this->create_publisher("output", 1); sub_ = this->create_subscription( "input", 1, std::bind(&PathToTrajectory::callback, this, _1)); + + this->declare_parameter("deceleration", -10.0); + this->declare_parameter("stop_offset", 3.0); + this->declare_parameter("max_speed", 30.0); + this->declare_parameter("traj_width", 1.0); + + deceleration_ = this->get_parameter("deceleration").as_double(); + stop_offset_ = this->get_parameter("stop_offset").as_double(); + max_speed_ = this->get_parameter("max_speed").as_double(); + traj_width_ = this->get_parameter("traj_width").as_double(); } void PathToTrajectory::callback(const PathWithLaneId::SharedPtr msg) { @@ -31,6 +41,22 @@ void PathToTrajectory::callback(const PathWithLaneId::SharedPtr msg) { trajectory_point.longitudinal_velocity_mps = path_point_with_lane_id.point.longitudinal_velocity_mps; trajectory.points.emplace_back(std::move(trajectory_point)); } + const double stop_time= max_speed_/std::abs(deceleration_); + const double dec_mpss=deceleration_/3.6; + const double speed_mps=max_speed_/3.6; + const double stop_dis=0.5*dec_mpss*stop_time*stop_time+speed_mps*stop_time+stop_offset_; + const int offset_index=stop_dis/traj_width_; + double v=speed_mps; + for(int i=0 ; i= 0 && index < trajectory.points.size()) { + trajectory.points.at(index).longitudinal_velocity_mps + = std::min(float(v), trajectory.points.at(index).longitudinal_velocity_mps); + } + } pub_->publish(trajectory); } diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index b4406f6a..20200939 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -78,18 +78,9 @@ void SimplePurePursuit::onTimer() // get closest trajectory point from current position TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); - // calc longitudinal speed and acceleration - double target_longitudinal_vel = - use_external_target_vel_ ? external_target_vel_ : closet_traj_point.longitudinal_velocity_mps; - double current_longitudinal_vel = odometry_->twist.twist.linear.x; - - cmd.longitudinal.speed = target_longitudinal_vel; - cmd.longitudinal.acceleration = - speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); - // calc lateral control //// calc lookahead distance - double lookahead_distance = lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_; + double lookahead_distance = lookahead_gain_ * closet_traj_point.longitudinal_velocity_mps + lookahead_min_distance_; //// calc center coordinate of rear wheel double rear_x = odometry_->pose.pose.position.x - wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z); @@ -107,6 +98,15 @@ void SimplePurePursuit::onTimer() } double lookahead_point_x = lookahead_point_itr->pose.position.x; double lookahead_point_y = lookahead_point_itr->pose.position.y; + + // calc longitudinal speed and acceleration + double target_longitudinal_vel = + use_external_target_vel_ ? external_target_vel_ : lookahead_point_itr->longitudinal_velocity_mps; + double current_longitudinal_vel = odometry_->twist.twist.linear.x; + + cmd.longitudinal.speed = target_longitudinal_vel; + cmd.longitudinal.acceleration = + speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); { // publish lookahead point marker auto marker_msg = Marker(); @@ -133,10 +133,15 @@ void SimplePurePursuit::onTimer() mkr_cmd_->publish(marker_msg); } // calc steering angle for lateral control - double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - - tf2::getYaw(odometry_->pose.pose.orientation); - cmd.lateral.steering_tire_angle = - steering_tire_angle_gain_ * std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance); + if (std::hypot(lookahead_point_x - rear_x, lookahead_point_y - rear_y) < lookahead_min_distance_) { + cmd.lateral.steering_tire_angle = 0.0; + } else { + double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - + tf2::getYaw(odometry_->pose.pose.orientation); + cmd.lateral.steering_tire_angle = + steering_tire_angle_gain_ * + std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance); + } } pub_cmd_->publish(cmd); cmd.lateral.steering_tire_angle /= steering_tire_angle_gain_; diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index ba3f9b7b..71b99a61 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -8,11 +8,10 @@ Panels: - /Planning1/ScenarioPlanning1/ScenarioTrajectory1 - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1 - /Planning1/Costmap1/Map1 - - /Planning1/MPPI1/Trajectory1 - - /Planning1/MPPI1/ReferenceTrajectory1 + - /Planning1/MPPI1 - /Planning1/MPPI1/ReferenceTrajectory1/View Path1 Splitter Ratio: 0.557669460773468 - Tree Height: 158 + Tree Height: 140 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -725,12 +724,7 @@ Visualization Manager: Color: 119; 11; 32 Name: PredictedObjects Namespaces: - acceleration: true - label: true - shape: true - twist: true - uuid: true - velocity: true + {} PEDESTRIAN: Alpha: 0.9990000128746033 Color: 255; 192; 203 @@ -833,11 +827,7 @@ Visualization Manager: Enabled: true Name: RouteArea Namespaces: - goal_lanelets: true - lane_start_bound: false - left_lane_bound: false - right_lane_bound: false - route_lanelets: true + {} Topic: Depth: 5 Durability Policy: Transient Local @@ -1240,8 +1230,7 @@ Visualization Manager: Enabled: true Name: Bound Namespaces: - left_bound: true - right_bound: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -2012,7 +2001,7 @@ Visualization Manager: Value: true - Class: rviz_plugins/Trajectory Color Border Vel Max: 3 - Enabled: true + Enabled: false Name: ReferenceTrajectory Topic: Depth: 5 @@ -2021,7 +2010,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/debug/path - Value: true + Value: false View Footprint: Alpha: 1 Color: 230; 230; 50 @@ -2128,7 +2117,7 @@ Visualization Manager: Enabled: true Name: MarkerArray Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -2311,12 +2300,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1043 + Height: 963 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -2325,6 +2314,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 - Y: 32 + Width: 1440 + X: 240 + Y: 112 From 55191cf619d4c5f3ed04be9d8a47267da926d573 Mon Sep 17 00:00:00 2001 From: sitahara <33857284+sitahara@users.noreply.github.com> Date: Wed, 9 Oct 2024 19:20:09 +0900 Subject: [PATCH 44/48] Feat (control): separate control modes (#54) * draft: separate control modes * chore (control): avoiding merge conflict * chore(control): avoiding merge conflict 2 * feat (control): added steering gain functionality from official branch * feat (control): fully implement lateral stanley control * chore (control): comment out unused variable * feat (contol): made extra steering gain tunable in pure pursuit * feat (control): enabled new control interface in control.launch.xml * Chore (control): Fix spelling errors --------- Co-authored-by: Shotaro Itahara --- .../launch/components/control.launch.xml | 47 ++++- .../CMakeLists.txt | 16 ++ .../ackermann_control_publisher.hpp | 33 ++++ .../ackermann_control_publisher/package.xml | 22 +++ .../src/ackermann_control_publisher.cpp | 55 ++++++ .../lateral_pure_pursuit/CMakeLists.txt | 16 ++ .../lateral_pure_pursuit.hpp | 81 +++++++++ .../lateral/lateral_pure_pursuit/package.xml | 23 +++ .../src/lateral_pure_pursuit.cpp | 171 ++++++++++++++++++ .../lateral/lateral_stanley/CMakeLists.txt | 16 ++ .../lateral_stanley/lateral_stanley.hpp | 86 +++++++++ .../lateral/lateral_stanley/package.xml | 26 +++ .../lateral_stanley/src/lateral_stanley.cpp | 163 +++++++++++++++++ .../simple_pd_speed_control/CMakeLists.txt | 16 ++ .../simple_pd_speed_control.hpp | 66 +++++++ .../simple_pd_speed_control/package.xml | 23 +++ .../src/simple_pd_speed_control.cpp | 95 ++++++++++ 17 files changed, 946 insertions(+), 9 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/include/ackermann_control_publisher/ackermann_control_publisher.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/src/ackermann_control_publisher.cpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/include/lateral_pure_pursuit/lateral_pure_pursuit.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/src/lateral_pure_pursuit.cpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/include/lateral_stanley/lateral_stanley.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/src/lateral_stanley.cpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/include/simple_pd_speed_control/simple_pd_speed_control.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index d5d98a9c..44181f68 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -10,9 +10,9 @@ - - + @@ -20,12 +20,12 @@ - + - + --> - @@ -35,10 +35,39 @@ + --> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/CMakeLists.txt new file mode 100644 index 00000000..d1c55a04 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(ackermann_control_publisher) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_executable(ackermann_control_publisher_node + src/ackermann_control_publisher.cpp +) + +ament_auto_package() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/include/ackermann_control_publisher/ackermann_control_publisher.hpp b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/include/ackermann_control_publisher/ackermann_control_publisher.hpp new file mode 100644 index 00000000..9ff61ead --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/include/ackermann_control_publisher/ackermann_control_publisher.hpp @@ -0,0 +1,33 @@ +#include +#include +#include + +namespace ackermann_control_publisher { + using namespace std::literals::chrono_literals; + using std_msgs::msg::Float64; + using autoware_auto_control_msgs::msg::AckermannControlCommand; + + class AckermannControlPublisher : public rclcpp::Node { + public: + explicit AckermannControlPublisher(); + + // subscribers + rclcpp::Subscription::SharedPtr sub_longitudinal_; + rclcpp::Subscription::SharedPtr sub_lateral_; + + // publishers + rclcpp::Publisher::SharedPtr pub_cmd_; + + // timer for control + rclcpp::TimerBase::SharedPtr timer_; + + private: + + // control command + double longitudinal_acc_; // 目標加速度 [m/s^2] + double lateral_tire_angle_; // タイヤ舵角 [rad] + + void onTimer(); + bool subscribeMessageAvailable(); + }; +} //namespace ackermann_control_publisher \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/package.xml b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/package.xml new file mode 100644 index 00000000..1210e345 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/package.xml @@ -0,0 +1,22 @@ + + + + ackermann_control_publisher + 0.0.1 + Bundles individual lateral/longitudinal commands and publishes them as `AckermannControlCommand` message + Shotaro Itahara + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + autoware_auto_control_msgs + std_msgs + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/src/ackermann_control_publisher.cpp b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/src/ackermann_control_publisher.cpp new file mode 100644 index 00000000..aff15788 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/src/ackermann_control_publisher.cpp @@ -0,0 +1,55 @@ +#include "ackermann_control_publisher/ackermann_control_publisher.hpp" + +namespace ackermann_control_publisher{ + AckermannControlPublisher::AckermannControlPublisher(): Node("ackermann_control_publisher"){ + + pub_cmd_ = create_publisher("output/ackermann_command", 1); + + sub_longitudinal_ = create_subscription( + "input/longitudinal", 1, [this](const Float64::SharedPtr msg) { longitudinal_acc_ = msg->data; }); + sub_lateral_ = create_subscription( + "input/lateral", 1, [this](const Float64::SharedPtr msg) { lateral_tire_angle_ = msg->data; }); + + using namespace std::literals::chrono_literals; + timer_ = + rclcpp::create_timer(this, get_clock(), 5ms, std::bind(&AckermannControlPublisher::onTimer, this)); // 200Hz + + } + + void AckermannControlPublisher::onTimer(){ + // check data + if (!subscribeMessageAvailable()) { + return; + } + rclcpp::Time stamp = get_clock()->now(); + + AckermannControlCommand cmd = AckermannControlCommand(); + cmd.stamp = stamp; + cmd.longitudinal.stamp = stamp; + cmd.lateral.stamp = stamp; + + cmd.longitudinal.acceleration = longitudinal_acc_; + cmd.lateral.steering_tire_angle = lateral_tire_angle_; + + pub_cmd_->publish(cmd); + } + + bool AckermannControlPublisher::subscribeMessageAvailable(){ + if (!longitudinal_acc_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available"); + return false; + } + if (!lateral_tire_angle_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available"); + return false; + } + return true; + } +} + +int main(int argc, char const * argv[]){ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/CMakeLists.txt new file mode 100644 index 00000000..ad474e00 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(lateral_pure_pursuit) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_executable(lateral_pure_pursuit_node + src/lateral_pure_pursuit.cpp +) + +ament_auto_package() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/include/lateral_pure_pursuit/lateral_pure_pursuit.hpp b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/include/lateral_pure_pursuit/lateral_pure_pursuit.hpp new file mode 100644 index 00000000..3a3e1cb7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/include/lateral_pure_pursuit/lateral_pure_pursuit.hpp @@ -0,0 +1,81 @@ +// Copyright 2024 Booars +// +// 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 LATERAL_PURE_PURSUIT_HPP_ +#define LATERAL_PURE_PURSUIT_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace lateral_pure_pursuit{ + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Pose; + using geometry_msgs::msg::Twist; + using geometry_msgs::msg::Vector3Stamped; + using visualization_msgs::msg::Marker; + using std_msgs::msg::Float64; + using nav_msgs::msg::Odometry; + + class LateralPurePursuit : public rclcpp::Node { + public: + explicit LateralPurePursuit(); + + // subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + // publishers + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // timer for control + rclcpp::TimerBase::SharedPtr timer_; + + // updated by subscribers + Trajectory::SharedPtr trajectory_; + Odometry::SharedPtr odometry_; + + + + + private: + void onTimer(); + bool subscribeMessageAvailable(); + + //parameters + double wheel_base_; // Distance between front and rear axle, needed for geometric control + double lookahead_gain_; // Determines how the lookahead distance grows in relationship with speed + double lookahead_min_distance_; // Minimum lookahead distance, any lookahead distance will be built on top of this + double extra_steering_gain_; // Duct tape fix constant to account for lack of steering motor power + + OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters); + }; +} // namespace lateral_pure_pursuit + + +#endif //LATERAL_PURE_PURSUIT_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/package.xml b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/package.xml new file mode 100644 index 00000000..87a82ef7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/package.xml @@ -0,0 +1,23 @@ + + + + lateral_pure_pursuit + 0.0.1 + Pure pursuit control, takes desired trajectory and odometry, returns steering angle only + Shotaro Itahara + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + autoware_auto_control_msgs + autoware_auto_planning_msgs + motion_utils + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/src/lateral_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/src/lateral_pure_pursuit.cpp new file mode 100644 index 00000000..edafc893 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/src/lateral_pure_pursuit.cpp @@ -0,0 +1,171 @@ +// Copyright 2024 Booars +// +// 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 "lateral_pure_pursuit/lateral_pure_pursuit.hpp" +#include +#include + +#include + +namespace lateral_pure_pursuit{ + + using motion_utils::findNearestIndex; + using tier4_autoware_utils::calcLateralDeviation; + using tier4_autoware_utils::calcYawDeviation; + + LateralPurePursuit::LateralPurePursuit(): Node("lateral_pure_pursuit"), + // initialize parameters + wheel_base_(declare_parameter("wheel_base", 2.14)), + lookahead_gain_(declare_parameter("lookahead_gain", 1.0)), + lookahead_min_distance_(declare_parameter("lookahead_min_distance", 1.0)), + extra_steering_gain_(declare_parameter("steering_tire_angle_gain",1.0)){ + + pub_cmd_ = create_publisher("output/steer_angle", 1); + pub_marker_ = create_publisher("debug/pursuit_lookahead2", 1); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); + sub_trajectory_ = create_subscription( + "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); + + using namespace std::literals::chrono_literals; + timer_ = + rclcpp::create_timer(this, get_clock(), 30ms, std::bind(&LateralPurePursuit::onTimer, this)); + + // dynamic reconfigure + auto parameter_change_cb = std::bind(&LateralPurePursuit::parameter_callback, this, std::placeholders::_1); + reset_param_handler_ = LateralPurePursuit::add_on_set_parameters_callback(parameter_change_cb); + + // steering gain + // todo: make this configurable from dynamic reconfigure + } + + void LateralPurePursuit::onTimer(){ + // check data + if (!subscribeMessageAvailable()) { + return; + } + + size_t closet_traj_point_idx = + findNearestIndex(trajectory_->points, odometry_->pose.pose.position); + + Float64 cmd_msg = Float64(); + if ( + (closet_traj_point_idx == trajectory_->points.size() - 1) || + (trajectory_->points.size() <= 5)) { + cmd_msg.data = 0.0; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal"); + } else { + // get closest trajectory point from current position + // double current_longitudinal_vel = odometry_->twist.twist.linear.x; + double current_longitudinal_vel = 8.0; + + // calc lateral control + //// calc lookahead distance + double lookahead_distance = lookahead_gain_ * current_longitudinal_vel + lookahead_min_distance_; + //// calc center coordinate of rear wheel + ////// assuming odom is in the middle of the car + double rear_x = odometry_->pose.pose.position.x - + wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z); + double rear_y = odometry_->pose.pose.position.y - + wheel_base_ / 2.0 * std::sin(odometry_->pose.pose.orientation.z); + //// search lookahead point + // * todo: change this so that the actual distance on the track is considered + auto lookahead_point_itr = std::find_if( + trajectory_->points.begin() + closet_traj_point_idx, trajectory_->points.end(), + [&](const TrajectoryPoint & point) { + return std::hypot(point.pose.position.x - rear_x, point.pose.position.y - rear_y) >= + lookahead_distance; + }); + if (lookahead_point_itr == trajectory_->points.end()) { + lookahead_point_itr = trajectory_->points.end() - 1; + } + double lookahead_point_x = lookahead_point_itr->pose.position.x; + double lookahead_point_y = lookahead_point_itr->pose.position.y; + + // publish lookahead point marker + { + auto marker_msg = Marker(); + marker_msg.header.frame_id = "map"; + marker_msg.header.stamp = now(); + marker_msg.ns = "basic_shapes"; + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::SPHERE; + marker_msg.action = visualization_msgs::msg::Marker::ADD; + marker_msg.pose.position.x = lookahead_point_x; + marker_msg.pose.position.y = lookahead_point_y; + marker_msg.pose.position.z = 80; + marker_msg.pose.orientation.x = 0.0; + marker_msg.pose.orientation.y = 0.0; + marker_msg.pose.orientation.z = 0.0; + marker_msg.pose.orientation.w = 1.0; + marker_msg.scale.x = 3.0; + marker_msg.scale.y = 3.0; + marker_msg.scale.z = 3.0; + marker_msg.color.r = 1.0f; + marker_msg.color.g = 0.0f; + marker_msg.color.b = 0.0f; + marker_msg.color.a = 1.0; + pub_marker_->publish(marker_msg); + } + // calc steering angle for lateral control + double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - + tf2::getYaw(odometry_->pose.pose.orientation); + cmd_msg.data = + std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance) * extra_steering_gain_; + } + pub_cmd_->publish(cmd_msg); + } + + bool LateralPurePursuit::subscribeMessageAvailable(){ + if (!odometry_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available"); + return false; + } + if (!trajectory_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available"); + return false; + } + return true; + } + rcl_interfaces::msg::SetParametersResult LateralPurePursuit::parameter_callback(const std::vector ¶meters){ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "lookahead_gain") { + lookahead_gain_ = parameter.as_double(); + RCLCPP_INFO(LateralPurePursuit::get_logger(), "lookahead_gain changed to %f", lookahead_gain_); + } + else if (parameter.get_name() == "lookahead_min_distance") { + lookahead_min_distance_ = parameter.as_double(); + RCLCPP_INFO(LateralPurePursuit::get_logger(), "lookahead_min_distance changed to %f", lookahead_min_distance_); + } + else if (parameter.get_name() == "steering_tire_angle_gain") { + extra_steering_gain_ = parameter.as_double(); + RCLCPP_INFO(LateralPurePursuit::get_logger(), "steering_tire_angle_gain changed to %f", extra_steering_gain_); + } + } + return result; + } +} // namespace lateral_pure_pursuit + + + +int main(int argc, char const * argv[]){ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/CMakeLists.txt new file mode 100644 index 00000000..e9786438 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(lateral_stanley) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_executable(lateral_stanley_node + src/lateral_stanley.cpp +) + +ament_auto_package() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/include/lateral_stanley/lateral_stanley.hpp b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/include/lateral_stanley/lateral_stanley.hpp new file mode 100644 index 00000000..0d565cd8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/include/lateral_stanley/lateral_stanley.hpp @@ -0,0 +1,86 @@ +// Copyright 2024 Booars +// +// 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 LATERAL_STANLEY_HPP_ +#define LATERAL_STANLEY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace lateral_stanley{ + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Pose; + using geometry_msgs::msg::Twist; + using geometry_msgs::msg::Vector3Stamped; + using visualization_msgs::msg::Marker; + using std_msgs::msg::Float64; + using nav_msgs::msg::Odometry; + + class LateralStanley : public rclcpp::Node { + public: + explicit LateralStanley(); + + // subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + // tf2 listeners + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener; + + // publishers + rclcpp::Publisher::SharedPtr pub_angle_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // timer for control + rclcpp::TimerBase::SharedPtr timer_; + + // updated by subscribers + Trajectory::SharedPtr trajectory_; + Odometry::SharedPtr odometry_; + + // stanley parameters + double k_gain; // Gain to determine additional steering based on speed + double k_gain_slow; // Constant to prevent zero division + double steering_tire_angle_gain; // Duct tape fix constant to account for lack of steering motor power + + private: + void onTimer(); + bool subscribeMessageAvailable(); + + // control variables + double error_distance_; + double track_yaw_; + double result_angle_; + + OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters); + }; +} // namespace lateral_stanley + + +#endif //LATERAL_STANLEY_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/package.xml b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/package.xml new file mode 100644 index 00000000..d2ecde5d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/package.xml @@ -0,0 +1,26 @@ + + + + lateral_stanley + 0.0.1 + Stanley control, takes desired trajectory and odometry, returns steering angle only + Shotaro Itahara + Apache-2.0 + + ament_cmake + + rclcpp + autoware_auto_control_msgs + autoware_auto_planning_msgs + motion_utils + geometry_msgs + std_msgs + nav_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/src/lateral_stanley.cpp b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/src/lateral_stanley.cpp new file mode 100644 index 00000000..9ac6df83 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_stanley/src/lateral_stanley.cpp @@ -0,0 +1,163 @@ +// Copyright 2024 Booars +// +// 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 "lateral_stanley/lateral_stanley.hpp" + +#include +#include + +#include + +#include + +namespace lateral_stanley{ + + using motion_utils::findNearestIndex; + + LateralStanley::LateralStanley(): Node("lateral_stanley"), tf_buffer(this->get_clock()), tf_listener(tf_buffer), + k_gain(declare_parameter("k_gain", 2.0)), + k_gain_slow(declare_parameter("k_gain_slow", 1.0)), + steering_tire_angle_gain(declare_parameter("steering_tire_angle_gain",1.0)){ + pub_marker_ = create_publisher("debug/forward_point", 1); + + pub_angle_ = create_publisher("output/steer_angle", 1); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); + sub_trajectory_ = create_subscription( + "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); + + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer(this, get_clock(), 10ms, std::bind(&LateralStanley::onTimer, this)); + + // dynamic reconfigure + auto parameter_change_cb = std::bind(&LateralStanley::parameter_callback, this, std::placeholders::_1); + reset_param_handler_ = LateralStanley::add_on_set_parameters_callback(parameter_change_cb); + } + + void LateralStanley::onTimer(){ + // check data + if (!subscribeMessageAvailable()) { + return; + } + + // Check if goal is "reached" + size_t closet_traj_point_idx = findNearestIndex(trajectory_->points, odometry_->pose.pose.position); + if ((closet_traj_point_idx == trajectory_->points.size() - 1) || (trajectory_->points.size() <= 5)) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal"); + } + // Otherwise (main control) + else { + // get closest trajectory point from current position + TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); + + if (trajectory_->points.size() - (closet_traj_point_idx+1) <= 3 ){ + // too close to the end (shouldn't happen) + result_angle_=0.0; + } + else{ + // obtain closest point and next point on the trajectory + size_t next_closest_traj_point_idx = closet_traj_point_idx+1; + TrajectoryPoint next_closet_traj_point = trajectory_->points.at(next_closest_traj_point_idx); + + // calculate track "yaw" angle + //// Trajectory vector + double x_track = next_closet_traj_point.pose.position.x - closet_traj_point.pose.position.x; + double y_track = next_closet_traj_point.pose.position.y - closet_traj_point.pose.position.y; + + double norm_track = sqrt(pow(x_track,2) + pow(y_track,2)); + + //// vehicle heading vector in "map" frame + //// obtain this by transforming constant vector in "base_link" frame to "map" frame + Vector3Stamped vector_fixed, vector_vehicle; + double norm_vehicle=1.0; + { + vector_fixed.header.frame_id = "base_link"; + vector_fixed.vector.x = 1.0; + vector_fixed.vector.y = 0.0; + vector_fixed.vector.z = 0.0; + } + try{ + auto transform = tf_buffer.lookupTransform("map","base_link", tf2::TimePointZero); + tf2::doTransform(vector_fixed, vector_vehicle, transform); + norm_vehicle = sqrt(pow(vector_vehicle.vector.x, 2) + pow(vector_vehicle.vector.y, 2)); + } + catch (tf2::TransformException &ex){ + RCLCPP_ERROR(get_logger(), "Could not obtain transform from map to base_link: %s", ex.what()); + } + + // calculate the cosine of two vectors, from the inner product + // use outer product to calculate signed angle (if v1xv2 >=0, then v2 is "to the left" of v1 ) + double ip_vector = x_track*vector_vehicle.vector.x + y_track*vector_vehicle.vector.y; + double cos_vector = ip_vector / (norm_track * norm_vehicle); + double angle = acos(cos_vector); + + double xp_vector = x_track*vector_vehicle.vector.y - y_track*vector_vehicle.vector.x; + if(xp_vector>=0) angle*=-1; + + // calculate positional error from nearest trajectory point + + error_distance_ = sqrt( + pow(closet_traj_point.pose.position.x-odometry_->pose.pose.position.x,2) + + pow(closet_traj_point.pose.position.y-odometry_->pose.pose.position.y,2)); + if ( + (closet_traj_point.pose.position.x-odometry_->pose.pose.position.x)*vector_vehicle.vector.y + - (closet_traj_point.pose.position.y-odometry_->pose.pose.position.y)*vector_vehicle.vector.x >= 0){ + error_distance_*=-1; + } + track_yaw_ = atan2(k_gain*error_distance_, odometry_->twist.twist.linear.x+k_gain_slow); + result_angle_ = angle+track_yaw_; + } + } + Float64 pub_angle = Float64(); + pub_angle.data = result_angle_ * steering_tire_angle_gain; + pub_angle_->publish(pub_angle); + } + + bool LateralStanley::subscribeMessageAvailable(){ + if (!odometry_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available"); + return false; + } + if (!trajectory_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available"); + return false; + } + return true; + } + rcl_interfaces::msg::SetParametersResult LateralStanley::parameter_callback(const std::vector ¶meters){ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "k_gain") { + k_gain = parameter.as_double(); + RCLCPP_INFO(LateralStanley::get_logger(), "k_gain changed to %f", k_gain); + } else if (parameter.get_name() == "k_gain_slow") { + k_gain_slow = parameter.as_double(); + RCLCPP_INFO(LateralStanley::get_logger(), "k_gain_slow changed to %f", k_gain_slow); + } else if (parameter.get_name() == "steering_tire_angle_gain"){ + steering_tire_angle_gain = parameter.as_double(); + RCLCPP_INFO(LateralStanley::get_logger(), "steering_tire_angle_gain changed to %f", steering_tire_angle_gain); + } + } + return result; + } +} // namespace lateral_stanley + +int main(int argc, char const * argv[]){ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/CMakeLists.txt new file mode 100644 index 00000000..be070f66 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(simple_pd_speed_control) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_executable(simple_pd_speed_control_node + src/simple_pd_speed_control.cpp +) + +ament_auto_package() \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/include/simple_pd_speed_control/simple_pd_speed_control.hpp b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/include/simple_pd_speed_control/simple_pd_speed_control.hpp new file mode 100644 index 00000000..f0ed98c8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/include/simple_pd_speed_control/simple_pd_speed_control.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 Booars +// +// 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 SIMPLE_SPEED_PD_CONTROL_HPP_ +#define SIMPLE_SPEED_PD_CONTROL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace simple_speed_pd_control { + +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using std_msgs::msg::Float64; + +class SimpleSpeedPDControl : public rclcpp::Node { + public: + explicit SimpleSpeedPDControl(); + + // subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + // publishers + rclcpp::Publisher::SharedPtr pub_cmd_; + + // timer + rclcpp::TimerBase::SharedPtr timer_; + + // updated by subscribers + Trajectory::SharedPtr trajectory_; + Odometry::SharedPtr odometry_; + + // speed control parameters + double speed_proportional_gain_; + OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; + + private: + void onTimer(); + bool subscribeMessageAvailable(); + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters); +}; + +} // namespace simple_speed_pd_control + +#endif // SIMPLE_SPEED_PD_CONTROL_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/package.xml b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/package.xml new file mode 100644 index 00000000..ff528a8b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/package.xml @@ -0,0 +1,23 @@ + + + + simple_pd_speed_control + 0.0.1 + PD control of vehicle acceleration from desired speed + Shotaro Itahara + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + autoware_auto_control_msgs + autoware_auto_planning_msgs + motion_utils + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp new file mode 100644 index 00000000..0240e107 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp @@ -0,0 +1,95 @@ +// Copyright 2024 Booars +// +// 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 "simple_pd_speed_control/simple_pd_speed_control.hpp" + +#include + +#include + +namespace simple_speed_pd_control{ + + using motion_utils::findNearestIndex; + + SimpleSpeedPDControl::SimpleSpeedPDControl() + : Node("simple_speed_pd_control"), speed_proportional_gain_(declare_parameter("speed_proportional_gain", 1.0)){ + pub_cmd_ = create_publisher("output/target_acc", 1); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); + sub_trajectory_ = create_subscription( + "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); + + using namespace std::literals::chrono_literals; + timer_ = + rclcpp::create_timer(this, get_clock(), 10ms, std::bind(&SimpleSpeedPDControl::onTimer, this)); // 100Hz + + // dynamic reconfigure + auto parameter_change_cb = std::bind(&SimpleSpeedPDControl::parameter_callback, this, std::placeholders::_1); + reset_param_handler_ = SimpleSpeedPDControl::add_on_set_parameters_callback(parameter_change_cb); + } + + void SimpleSpeedPDControl::onTimer(){ + // check data + if (!subscribeMessageAvailable()) { + return; + } + + // size_t closet_traj_point_idx = findNearestIndex(trajectory_->points, odometry_->pose.pose.position); + + // get closest trajectory point from current position + // TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); + + // calc longitudinal speed and acceleration + // double target_longitudinal_vel = closet_traj_point.longitudinal_velocity_mps; + double target_longitudinal_vel = 8.0; + double current_longitudinal_vel = odometry_->twist.twist.linear.x; + Float64 msg = Float64(); + msg.data = speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); + + pub_cmd_->publish(msg); + } + + bool SimpleSpeedPDControl::subscribeMessageAvailable(){ + if (!odometry_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available"); + return false; + } + if (!trajectory_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available"); + return false; + } + return true; + } + rcl_interfaces::msg::SetParametersResult SimpleSpeedPDControl::parameter_callback(const std::vector ¶meters){ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "speed_proportional_gain") { + speed_proportional_gain_ = parameter.as_double(); + RCLCPP_INFO(SimpleSpeedPDControl::get_logger(), "speed_proportional_gain changed to %f", speed_proportional_gain_); + } + } + return result; + } +} // namespace simple_speed_pd_control + + + +int main(int argc, char const * argv[]){ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 0552064663e74da398993bb6587873d58fee42a5 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 9 Oct 2024 19:33:28 +0900 Subject: [PATCH 45/48] update lanelet2_map.osm in booars_launch with aichallenge_submit_launch's one Signed-off-by: Autumn60 --- .../booars_launch/map/lanelet2_map.osm | 2453 +++++++++-------- 1 file changed, 1330 insertions(+), 1123 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm index 49147919..0501b7a0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/lanelet2_map.osm @@ -1,6 +1,6 @@ - + @@ -1561,39 +1561,39 @@ - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + @@ -1746,29 +1746,29 @@ - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + @@ -2336,929 +2336,914 @@ - - - + + + - - - - + + + + - - - - + + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - + + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - - - - - + + + - - - + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - - - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - - - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - - + + + + - - - - + + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - + + + + - - - + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - - + + + + - - - + + + - - - - + + + + - - - - + + + + - - - - + + + + - - - + + + - - - - + + + + - - - - + + + + - - - + + + - - - + + + @@ -3281,57 +3266,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -3343,13 +3277,6 @@ - - - - - - - @@ -3365,6 +3292,11 @@ + + + + + @@ -3376,41 +3308,10 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + @@ -3484,20 +3385,6 @@ - - - - - - - - - - - - - - @@ -3525,197 +3412,10 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -3780,83 +3480,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -3926,115 +3549,699 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + \ No newline at end of file From abc641270b80f7754c4c39789f303c271065274b Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 9 Oct 2024 19:40:31 +0900 Subject: [PATCH 46/48] fix spelling in zenoh.json5 Signed-off-by: Autumn60 --- vehicle/zenoh.json5 | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vehicle/zenoh.json5 b/vehicle/zenoh.json5 index 6e8a7ffc..2209157f 100644 --- a/vehicle/zenoh.json5 +++ b/vehicle/zenoh.json5 @@ -86,7 +86,7 @@ //// - "integer" is a priority value in the range [1-7]. Highest priority is 1, lowest is 7 and default is 5. //// (see Zenoh Priority definition here: https://docs.rs/zenoh/latest/zenoh/publication/enum.Priority.html) //// - ":express" is an option to indicate that the Zenoh express policy must be used for those publications. - //// The express policy makes Zenoh to to send the message immediatly, not waiting for possible further messages + //// The express policy makes Zenoh to to send the message immediately, not waiting for possible further messages //// to create a bigger batch of messages. This usually has a positive impact on latency for the topic //// but a negative impact on the general throughput, as more overhead is used for those topics. pub_priorities: ["/joy=1:express"], @@ -96,7 +96,7 @@ //// routed to zenoh using the CongestionControl::Block option. //// Meaning the routing will be blocked in case of network congestion, //// blocking the DDS Reader and the RELIABLE DDS Writer in return. - //// When false (or for BERST_EFFORT DDS Writers), CongestionControl::Drop + //// When false (or for BEST_EFFORT DDS Writers), CongestionControl::Drop //// is used, meaning the route might drop some data in case of congestion. //// // reliable_routes_blocking: true, @@ -195,7 +195,7 @@ //}, //// - //// Configure the scouting mechanisms and their behaviours + //// Configure the scouting mechanisms and their behaviors //// //scouting: { // /// The UDP multicast scouting configuration. From d9e75df93a027dd9603f8e0887acec91055af926 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 9 Oct 2024 19:40:50 +0900 Subject: [PATCH 47/48] add words to .cspell.json for zenoh settings Signed-off-by: Autumn60 --- .cspell.json | 58 +++++++++++++++++++++++++++++++--------------------- 1 file changed, 35 insertions(+), 23 deletions(-) diff --git a/.cspell.json b/.cspell.json index df7e9be3..a42af469 100644 --- a/.cspell.json +++ b/.cspell.json @@ -54,8 +54,10 @@ "ackermann", "adapi", "aichallenge", + "argmin", "astar", "autocompute", + "autoconnect", "autodetermine", "automotiveaichallenge", "autoware", @@ -63,6 +65,8 @@ "booars", "brakemap", "buildtool", + "cind", + "coeffs", "colcon", "costmap", "cuda", @@ -70,13 +74,21 @@ "dallara", "dcmake", "decel", + "dind", "distro", "downsample", + "downsampling", + "dtheta", + "eguchi", "freespace", "gnss", "gnucxx", + "Golay", "gtest", + "Iceoryx", "initialpose", + "ints", + "Kohei", "lanechange", "lanefollowing", "lanelet", @@ -86,58 +98,58 @@ "linalg", "linestring", "linestrings", + "linkstate", "lowpass", "mapfile", "mathcal", "mgrs", + "michikuni", + "mppi", + "MPPI", + "mpss", + "multihop", + "ncourse", + "nodename", "odometry", "osrf", + "pinv", "pitstop", "pointcloud", + "Qdin", "rclcpp", "rclpy", "rcutils", + "reparameting", "rgba", "rois", + "rosbag", "rosdep", "rosdistro", + "rosout", "rrtstar", + "rsample", "rviz", + "satitzky", + "Savitzky", "schematypens", "sideshift", + "softplus", "srvs", "stddev", + "tempature", "traj", "urdf", + "vander", + "Vandermonde", "velodyne", "wextra", "wmctrl", "wpedantic", "xacro", + "xclip", "xyzrpy", "zcvf", - "mppi", - "Qdin", - "Kohei", - "MPPI", - "Savitzky", - "satitzky", - "Golay", - "Vandermonde", - "michikuni", - "eguchi", - "vander", - "pinv", - "dtheta", - "cind", - "dind", - "ncourse", - "argmin", - "reparameting", - "tempature", - "rsample", - "coeffs", - "softplus", - "mpss" + "zenoh", + "zenohd" ] } From de5e51b8f7b7a80b25bc3ae0d7fe6e51547de62e Mon Sep 17 00:00:00 2001 From: Michikuni Eguchi Date: Wed, 9 Oct 2024 22:17:01 +0900 Subject: [PATCH 48/48] mppi dynamic reconfigure (#55) --- .../planning/costmap_generator.param.yaml | 2 +- .../planning/mppi_controller.param.yaml | 18 +- .../launch/components/control.launch.xml | 4 +- .../mppi_controller/mppi_controller.py | 79 +++++++++ .../mppi_controller/mppi_controller_node.py | 30 +++- .../config/debug_sensing.rviz | 160 +++++++++++++++--- 6 files changed, 259 insertions(+), 34 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index 283e0a7f..62292907 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -13,7 +13,7 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" - inflation_radius: 1.4 # [m] + inflation_radius: 1.8 # [m] cached_costmap: min_x: 89607.0 # [m] max_x: 89687.0 # [m] diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml index 35ba01cc..50506a60 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml @@ -2,20 +2,20 @@ ros__parameters: # mppi horizon : 25 - num_samples : 4000 - u_min : [-4.0, -0.35] # accel(m/s2), steer angle(rad) - u_max : [3.0, 0.35] - sigmas : [2.0, 0.35] # sample range + num_samples : 5000 + u_min : [-4.0, -0.25] # accel(m/s2), steer angle(rad) + u_max : [2.0, 0.25] + sigmas : [2.0, 0.25] # sample range lambda : 1.0 - auto_lambda : false + auto_lambda : true # reference path DL : 0.1 lookahead_distance : 0.1 - reference_path_interval : 0.8 + reference_path_interval : 0.83 # cost weights - Qc : 10.0 - Ql : 1.0 - Qv : 4.0 + Qc : 20.0 + Ql : 5.0 + Qv : 0.5 Qo : 1000.0 Qin : 0.01 Qdin : 0.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index 44181f68..7c0c285a 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -12,8 +12,8 @@