diff --git a/.cspell.json b/.cspell.json new file mode 100644 index 00000000..3c1715b8 --- /dev/null +++ b/.cspell.json @@ -0,0 +1,109 @@ +{ + "ignorePaths": [ + "**/*.bash", + "**/*.cspell.json", + "**/*.dae", + "**/*.html", + "**/*.mp3", + "**/*.mp4", + "**/*.pcd", + "**/*.rviz", + "**/*.sh", + "**/*.stl", + "**/*.svg", + "**/*.wav", + "**/*.zip", + "**/.git/**", + "**/.gitignore", + "**/.vscode/**", + "**/build/**", + "**/CHANGELOG.rst", + "**/.clang-format", + "**/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)?( )?: .*$", "TODO\\((.*?)\\)"] + } + ], + "words": [ + "ackermann", + "adapi", + "aichallenge", + "autocompute", + "automotiveaichallenge", + "autoware", + "awsim", + "booars", + "buildtool", + "colcon", + "costmap", + "cuda", + "cyclonedds", + "dallara", + "dcmake", + "distro", + "downsample", + "freespace", + "gnss", + "gnucxx", + "gtest", + "initialpose", + "lanechange", + "lanefollowing", + "lanelet", + "lanelets", + "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" + ] +} 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 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 diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml new file mode 100644 index 00000000..bc7ea6ca --- /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: | + bot + sync-upstream + auto-merge-method: merge \ No newline at end of file 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 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 + + 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