From a2695fb054641eb09518b424cc766beb7c52624d Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 27 Jun 2024 20:45:06 +0900 Subject: [PATCH 01/11] add .gitignore Signed-off-by: Autumn60 --- .gitignore | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c2cb7e1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +# Visual Studio Code +.vscode/ +*.code-workspace + +# colcon +build/ +install/ +log/ + +# Python +*.pyc From c2493913520baba24bf1482fef41b042a8aa56a8 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 28 Jun 2024 19:49:44 +0900 Subject: [PATCH 02/11] create src dir Signed-off-by: Autumn60 --- src/.gitkeep | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/.gitkeep diff --git a/src/.gitkeep b/src/.gitkeep new file mode 100644 index 0000000..e69de29 From fb244bba0580a90e9e3bdad23b9528897e81e555 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 28 Jun 2024 19:49:52 +0900 Subject: [PATCH 03/11] add .repos file Signed-off-by: Autumn60 --- depend_packages.repos | 1 + 1 file changed, 1 insertion(+) create mode 100644 depend_packages.repos diff --git a/depend_packages.repos b/depend_packages.repos new file mode 100644 index 0000000..c22e013 --- /dev/null +++ b/depend_packages.repos @@ -0,0 +1 @@ +repositories: \ No newline at end of file From 02a0419e52ec90547e722b4e1a0c1a7fa1f6e399 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 28 Jun 2024 19:50:01 +0900 Subject: [PATCH 04/11] update README Signed-off-by: Autumn60 --- README.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/README.md b/README.md index b7cae96..df6abd8 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,39 @@ # wheel-stuck-ros-pkgs + Fool Stuck Robot + +## Requirements + +- Ubuntu 22.04 +- ROS 2 Humble + +## 1. Setup + +1. Update pkg list + ```bash + sudo apt update + ``` +2. Install VCS tool + ```bash + sudo apt install -y python3-vcstool + ``` +3. Clone repos + ```bash + git clone https://github.com/Fool-Stuck/wheel-stuck-ros-pkgs.git + ``` +4. Import depend pkgs(source) + ```bash + vcs import src < depend_packages.repos --recursive + ``` +5. Install depend pkgs(binary) + ```bash + rosdep install -i -y --from-paths src --ignore-src + ``` +6. Build + ```bash + colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release + ``` + +## 2. Run + +## LICENSE From 23c24cee7fe7a59440977b983d89752164ae64a1 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 28 Jun 2024 20:20:21 +0900 Subject: [PATCH 05/11] create dwa_planner pkg directories Signed-off-by: Autumn60 --- src/planning/dwa_planner/CMakeLists.txt | 24 +++++++++++++++++++++++ src/planning/dwa_planner/include/.gitkeep | 0 src/planning/dwa_planner/launch/.gitkeep | 0 src/planning/dwa_planner/package.xml | 18 +++++++++++++++++ src/planning/dwa_planner/src/.gitkeep | 0 5 files changed, 42 insertions(+) create mode 100644 src/planning/dwa_planner/CMakeLists.txt create mode 100644 src/planning/dwa_planner/include/.gitkeep create mode 100644 src/planning/dwa_planner/launch/.gitkeep create mode 100644 src/planning/dwa_planner/package.xml create mode 100644 src/planning/dwa_planner/src/.gitkeep diff --git a/src/planning/dwa_planner/CMakeLists.txt b/src/planning/dwa_planner/CMakeLists.txt new file mode 100644 index 0000000..3f4df3e --- /dev/null +++ b/src/planning/dwa_planner/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.5) +project(dwa_planner) + +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() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) \ No newline at end of file diff --git a/src/planning/dwa_planner/include/.gitkeep b/src/planning/dwa_planner/include/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/planning/dwa_planner/launch/.gitkeep b/src/planning/dwa_planner/launch/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/planning/dwa_planner/package.xml b/src/planning/dwa_planner/package.xml new file mode 100644 index 0000000..a4f557f --- /dev/null +++ b/src/planning/dwa_planner/package.xml @@ -0,0 +1,18 @@ + + + + dwa_planner + 0.0.0 + The __PACKAGE_NAME__ package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + ament_lint_auto + + + ament_cmake + + \ No newline at end of file diff --git a/src/planning/dwa_planner/src/.gitkeep b/src/planning/dwa_planner/src/.gitkeep new file mode 100644 index 0000000..e69de29 From c94449d822f71d6e9f78c44c9f5a0d0ed0078528 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 28 Jun 2024 20:42:51 +0900 Subject: [PATCH 06/11] add hpp and cpp file Signed-off-by: Autumn60 --- src/planning/dwa_planner/include/.gitkeep | 0 .../dwa_planner/include/dwa_planner/dwa_planner.hpp | 13 +++++++++++++ src/planning/dwa_planner/src/.gitkeep | 0 src/planning/dwa_planner/src/dwa_planner.cpp | 8 ++++++++ 4 files changed, 21 insertions(+) delete mode 100644 src/planning/dwa_planner/include/.gitkeep create mode 100644 src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp delete mode 100644 src/planning/dwa_planner/src/.gitkeep create mode 100644 src/planning/dwa_planner/src/dwa_planner.cpp diff --git a/src/planning/dwa_planner/include/.gitkeep b/src/planning/dwa_planner/include/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp b/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp new file mode 100644 index 0000000..9cb4256 --- /dev/null +++ b/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp @@ -0,0 +1,13 @@ +#ifndef DWA_PLANNER__DWA_PLANNER_HPP_ +#define DWA_PLANNER__DWA_PLANNER_HPP_ + +#include + +namespace dwa_planner { +class DWAPlanner : public rclcpp::Node { + public: + explicit DWAPlanner(const rclcpp::NodeOptions& options); +}; +} // namespace dwa_planner + +#endif \ No newline at end of file diff --git a/src/planning/dwa_planner/src/.gitkeep b/src/planning/dwa_planner/src/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/src/planning/dwa_planner/src/dwa_planner.cpp b/src/planning/dwa_planner/src/dwa_planner.cpp new file mode 100644 index 0000000..fa4ab0b --- /dev/null +++ b/src/planning/dwa_planner/src/dwa_planner.cpp @@ -0,0 +1,8 @@ +#include "dwa_planner/dwa_planner.hpp" + +namespace dwa_planner { + +DWAPlanner::DWAPlanner(const rclcpp::NodeOptions& options) + : Node("dwa_planner", options) {} + +} // namespace dwa_planner \ No newline at end of file From 6bbc6b7d7161fb2d5af082497bea8e5a7140bec1 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 28 Jun 2024 21:22:09 +0900 Subject: [PATCH 07/11] create NoCallSubscription wrapper class Signed-off-by: Autumn60 --- src/wheel_stuck_utils/CMakeLists.txt | 26 ++++++++++ .../ros/no_callback_subscription.hpp | 49 +++++++++++++++++++ src/wheel_stuck_utils/package.xml | 20 ++++++++ 3 files changed, 95 insertions(+) create mode 100644 src/wheel_stuck_utils/CMakeLists.txt create mode 100644 src/wheel_stuck_utils/include/wheel_stuck_utils/ros/no_callback_subscription.hpp create mode 100644 src/wheel_stuck_utils/package.xml diff --git a/src/wheel_stuck_utils/CMakeLists.txt b/src/wheel_stuck_utils/CMakeLists.txt new file mode 100644 index 0000000..728e713 --- /dev/null +++ b/src/wheel_stuck_utils/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.5) +project(wheel_stuck_utils) + +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(autoware_universe_utils SHARED + src/hoge.cpp +) + +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/src/wheel_stuck_utils/include/wheel_stuck_utils/ros/no_callback_subscription.hpp b/src/wheel_stuck_utils/include/wheel_stuck_utils/ros/no_callback_subscription.hpp new file mode 100644 index 0000000..1bab50d --- /dev/null +++ b/src/wheel_stuck_utils/include/wheel_stuck_utils/ros/no_callback_subscription.hpp @@ -0,0 +1,49 @@ +#ifndef WHEEL_STUCK_UTILS__ROS__NO_CALLBACK_SUBSCRIPTION_HPP_ +#define WHEEL_STUCK_UTILS__ROS__NO_CALLBACK_SUBSCRIPTION_HPP_ + +#include + +namespace wheel_stuck_utils { + +template +class NoCallbackSubscription { + private: + typename rclcpp::Subscription::SharedPtr subscription_; + + public: + using SharedPtr = std::shared_ptr>; + static SharedPtr create_subscription(rclcpp::Node* node, + const std::string& topic_name, + const rclcpp::QoS& qos = rclcpp::QoS{ + 1}) { + return std::make_shared>(node, topic_name, + qos); + } + + explicit NoCallbackSubscription(rclcpp::Node* node, + const std::string& topic_name, + const rclcpp::QoS& qos = rclcpp::QoS{1}) { + auto noexec_callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); + noexec_subscription_options.callback_group = noexec_callback_group; + + subscription_ = node->create_subscription( + topic_name, qos, + [node]([[maybe_unused]] const typename TMessage::ConstSharedPtr msg) { + assert(false); + }, + noexec_subscription_options); + } + + typename TMessage::ConstSharedPtr getData() { + auto data = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool is_received = subscription_->take(*data, message_info); + return is_received ? data : nullptr; + } +}; + +} // namespace wheel_stuck_utils + +#endif \ No newline at end of file diff --git a/src/wheel_stuck_utils/package.xml b/src/wheel_stuck_utils/package.xml new file mode 100644 index 0000000..09976be --- /dev/null +++ b/src/wheel_stuck_utils/package.xml @@ -0,0 +1,20 @@ + + + + wheel_stuck_utils + 0.1.0 + The wheel_stuck_utils package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + rclcpp + + ament_lint_auto + + + ament_cmake + + \ No newline at end of file From 24f4d0b58a4f5a04cb43396980e093cbaea2893f Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 28 Jun 2024 21:35:58 +0900 Subject: [PATCH 08/11] comment out unnecessary line in CMakeLists Signed-off-by: Autumn60 --- src/wheel_stuck_utils/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/wheel_stuck_utils/CMakeLists.txt b/src/wheel_stuck_utils/CMakeLists.txt index 728e713..c7adc3b 100644 --- a/src/wheel_stuck_utils/CMakeLists.txt +++ b/src/wheel_stuck_utils/CMakeLists.txt @@ -14,9 +14,9 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(autoware_universe_utils SHARED - src/hoge.cpp -) +# ament_auto_add_library(autoware_universe_utils SHARED +# src/source.cpp +# ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) From 14eceb562423ee6b92896769f972934a217c2e58 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sat, 29 Jun 2024 14:20:29 +0900 Subject: [PATCH 09/11] implement dwa_planner framework Signed-off-by: Autumn60 --- src/planning/dwa_planner/CMakeLists.txt | 10 + .../dwa_planner/config/dwa_planner.param.yaml | 11 + .../include/dwa_planner/dwa_planner.hpp | 115 ++++++++- src/planning/dwa_planner/launch/.gitkeep | 0 .../dwa_planner/launch/dwa_planner.launch.xml | 18 ++ src/planning/dwa_planner/package.xml | 5 + src/planning/dwa_planner/src/dwa_planner.cpp | 243 +++++++++++++++++- 7 files changed, 393 insertions(+), 9 deletions(-) create mode 100644 src/planning/dwa_planner/config/dwa_planner.param.yaml delete mode 100644 src/planning/dwa_planner/launch/.gitkeep create mode 100644 src/planning/dwa_planner/launch/dwa_planner.launch.xml diff --git a/src/planning/dwa_planner/CMakeLists.txt b/src/planning/dwa_planner/CMakeLists.txt index 3f4df3e..1cb7532 100644 --- a/src/planning/dwa_planner/CMakeLists.txt +++ b/src/planning/dwa_planner/CMakeLists.txt @@ -14,11 +14,21 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +ament_auto_add_library(dwa_planner SHARED + src/dwa_planner.cpp +) + +rclcpp_components_register_node(dwa_planner + PLUGIN "dwa_planner::DWAPlanner" + EXECUTABLE dwa_planner_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/src/planning/dwa_planner/config/dwa_planner.param.yaml b/src/planning/dwa_planner/config/dwa_planner.param.yaml new file mode 100644 index 0000000..886f67a --- /dev/null +++ b/src/planning/dwa_planner/config/dwa_planner.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + min_velocity: 0.0 + max_velocity: 0.5 + max_acceleration: 0.5 + max_angular_speed: 1.0 + max_angular_acceleration: 1.0 + prediction_time: 1.0 + velocity_resolution: 10 + angular_velocity_resolution: 10 diff --git a/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp b/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp index 9cb4256..154792d 100644 --- a/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp +++ b/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp @@ -2,12 +2,117 @@ #define DWA_PLANNER__DWA_PLANNER_HPP_ #include +#include -namespace dwa_planner { -class DWAPlanner : public rclcpp::Node { - public: - explicit DWAPlanner(const rclcpp::NodeOptions& options); +#include +#include +#include +#include + +#include +#include + +namespace dwa_planner +{ + +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; +using Odometry = nav_msgs::msg::Odometry; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using Twist = geometry_msgs::msg::Twist; +using TwistStamped = geometry_msgs::msg::TwistStamped; + +using OccupancyGridSubscription = wheel_stuck_utils::NoCallbackSubscription; +using OdometrySubscription = wheel_stuck_utils::NoCallbackSubscription; +using PoseStampedSubscription = wheel_stuck_utils::NoCallbackSubscription; +using TwistStampedPublisher = rclcpp::Publisher; + +class DWAPlanner : public rclcpp::Node +{ +private: + class State + { + public: + State( + const double x, const double y, const double yaw, const double velocity, + const double angular_velocity); + void simulate(const double velocity, const double angular_velocity, const double dt); + double x; + double y; + double yaw; + double velocity; + double angular_velocity; + }; + + class Window + { + public: + Window(); + Window( + const double min_velocity, const double max_velocity, const double min_angular_velocity, + const double max_angular_velocity); + double min_velocity; + double max_velocity; + double min_angular_velocity; + double max_angular_velocity; + }; + + using Trajectory = std::vector; + +public: + explicit DWAPlanner(const rclcpp::NodeOptions & options); + void update(); + + rclcpp::TimerBase::SharedPtr update_timer_; + +private: + bool subscribe_and_validate(); + bool try_subscribe_map(); + bool try_subscribe_odom(); + bool try_subscribe_goal(); + + Trajectory planning(); + Window generate_window(const Twist current_twist); + double calculate_stage_cost(const State & state); + double calculate_terminal_cost(const State & state); + + inline static double lerp(const double a, const double b, double t) + { + t = std::max(0.0, std::min(1.0, t)); + return a + t * (b - a); + } + + OccupancyGridSubscription::SharedPtr map_sub_; + OdometrySubscription::SharedPtr odom_sub_; + PoseStampedSubscription::SharedPtr goal_sub_; + TwistStampedPublisher::SharedPtr cmd_pub_; + + OccupancyGrid::ConstSharedPtr map_; + Odometry::ConstSharedPtr odom_; + PoseStamped::ConstSharedPtr goal_; + + rclcpp::Time last_map_sub_time_; + rclcpp::Time last_odom_sub_time_; + rclcpp::Time last_goal_sub_time_; + + double dt_; + double map_timeout_; + double odom_timeout_; + double goal_timeout_; + + double min_velocity_; + double max_velocity_; + double max_angular_speed_; + + double max_acceleration_; + double max_angular_acceleration_; + + double prediction_time_; + int velocity_resolution_; + int angular_velocity_resolution_; + + double velocity_resolution_inv_; + double angular_velocity_resolution_inv_; }; } // namespace dwa_planner -#endif \ No newline at end of file +#endif // DWA_PLANNER__DWA_PLANNER_HPP_ diff --git a/src/planning/dwa_planner/launch/.gitkeep b/src/planning/dwa_planner/launch/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/src/planning/dwa_planner/launch/dwa_planner.launch.xml b/src/planning/dwa_planner/launch/dwa_planner.launch.xml new file mode 100644 index 0000000..41a1c1f --- /dev/null +++ b/src/planning/dwa_planner/launch/dwa_planner.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/planning/dwa_planner/package.xml b/src/planning/dwa_planner/package.xml index a4f557f..93aef8a 100644 --- a/src/planning/dwa_planner/package.xml +++ b/src/planning/dwa_planner/package.xml @@ -10,6 +10,11 @@ ament_cmake_auto + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + wheel_stuck_utils ament_lint_auto diff --git a/src/planning/dwa_planner/src/dwa_planner.cpp b/src/planning/dwa_planner/src/dwa_planner.cpp index fa4ab0b..4f78e96 100644 --- a/src/planning/dwa_planner/src/dwa_planner.cpp +++ b/src/planning/dwa_planner/src/dwa_planner.cpp @@ -1,8 +1,243 @@ #include "dwa_planner/dwa_planner.hpp" -namespace dwa_planner { +namespace dwa_planner +{ -DWAPlanner::DWAPlanner(const rclcpp::NodeOptions& options) - : Node("dwa_planner", options) {} +DWAPlanner::State::State( + const double x, const double y, const double yaw, const double velocity, + const double angular_velocity) +: x(x), y(y), yaw(yaw), velocity(velocity), angular_velocity(angular_velocity) +{ +} -} // namespace dwa_planner \ No newline at end of file +void DWAPlanner::State::simulate( + const double velocity, const double angular_velocity, const double dt) +{ + x += velocity * std::cos(yaw) * dt; + y += velocity * std::sin(yaw) * dt; + yaw += angular_velocity * dt; + this->velocity = velocity; + this->angular_velocity = angular_velocity; +} + +DWAPlanner::Window::Window() +: min_velocity(0.0), max_velocity(0.0), min_angular_velocity(0.0), max_angular_velocity(0.0) +{ +} + +DWAPlanner::Window::Window( + const double min_velocity, const double max_velocity, const double min_angular_velocity, + const double max_angular_velocity) +: min_velocity(min_velocity), + max_velocity(max_velocity), + min_angular_velocity(min_angular_velocity), + max_angular_velocity(max_angular_velocity) +{ +} + +DWAPlanner::DWAPlanner(const rclcpp::NodeOptions & options) : Node("dwa_planner", options) +{ + // Declare Parameters + double update_rate_hz = this->declare_parameter("update_rate_hz", 20.0); + dt_ = 1.0 / update_rate_hz; + + map_timeout_ = this->declare_parameter("map_timeout", 1.0); + odom_timeout_ = this->declare_parameter("odom_timeout", 1.0); + goal_timeout_ = this->declare_parameter("goal_timeout", 1.0); + + min_velocity_ = this->declare_parameter("min_velocity", 0.0); + max_velocity_ = this->declare_parameter("max_velocity", 0.0); + max_acceleration_ = this->declare_parameter("max_acceleration", 0.0); + max_angular_speed_ = this->declare_parameter("max_angular_speed", 0.0); + max_angular_acceleration_ = this->declare_parameter("max_angular_acceleration", 0.0); + + prediction_time_ = this->declare_parameter("prediction_time", 1.0); + velocity_resolution_ = this->declare_parameter("velocity_resolution", 10); + angular_velocity_resolution_ = this->declare_parameter("angular_velocity_resolution", 10); + velocity_resolution_inv_ = 1.0 / velocity_resolution_; + angular_velocity_resolution_inv_ = 1.0 / angular_velocity_resolution_; + + // Declare Subscriptions + { + map_sub_ = OccupancyGridSubscription::create_subscription(this, "~/input/local_costmap"); + odom_sub_ = OdometrySubscription::create_subscription(this, "~/input/odom"); + goal_sub_ = PoseStampedSubscription::create_subscription(this, "~/input/local_goal"); + } + + // Declare Publishers + { + cmd_pub_ = this->create_publisher("~/output/cmd_vel", 1); + } + + // Declare timer for update function + { + auto update_callback = [this]() { this->update(); }; + auto period = + std::chrono::duration_cast(std::chrono::duration(dt_)); + update_timer_ = std::make_shared>( + this->get_clock(), period, std::move(update_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(update_timer_, nullptr); + } +} + +void DWAPlanner::update() +{ + if (!subscribe_and_validate()) return; + Trajectory best_trajectory = planning(); + + TwistStamped cmd; + { + cmd.header.frame_id = odom_->child_frame_id; + cmd.header.stamp = this->now(); + cmd.twist.linear.x = best_trajectory.front().velocity; + cmd.twist.angular.z = best_trajectory.front().angular_velocity; + } + + cmd_pub_->publish(cmd); +} + +bool DWAPlanner::subscribe_and_validate() +{ + bool isPlannerExecutable = true; + + // Subscribe map + { + if (try_subscribe_map()) last_map_sub_time_ = this->now(); + if (!map_ || (this->now() - last_map_sub_time_).seconds() > map_timeout_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No map received"); + isPlannerExecutable = false; + } + } + + // Subscribe odom + { + if (try_subscribe_odom()) last_odom_sub_time_ = this->now(); + if (!odom_ || (this->now() - last_odom_sub_time_).seconds() > odom_timeout_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No odom received"); + isPlannerExecutable = false; + } + } + + // Subscribe goal + { + if (try_subscribe_goal()) last_goal_sub_time_ = this->now(); + if (!goal_ || (this->now() - last_goal_sub_time_).seconds() > goal_timeout_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No goal received"); + isPlannerExecutable = false; + } + } + + if (!isPlannerExecutable) return false; + + if (map_->header.frame_id != odom_->header.frame_id) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Map and odom frame_id mismatch"); + isPlannerExecutable = false; + } + + if (goal_->header.frame_id != odom_->header.frame_id) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Goal and odom frame_id mismatch"); + isPlannerExecutable = false; + } + + return isPlannerExecutable; +} + +bool DWAPlanner::try_subscribe_map() +{ + auto map_msg = map_sub_->getData(); + if (!map_msg) return false; + map_ = map_msg; + return true; +} + +bool DWAPlanner::try_subscribe_odom() +{ + auto odom_msg = odom_sub_->getData(); + if (!odom_msg) return false; + odom_ = odom_msg; + return true; +} + +bool DWAPlanner::try_subscribe_goal() +{ + auto goal_msg = goal_sub_->getData(); + if (!goal_msg) return false; + goal_ = goal_msg; + return true; +} + +DWAPlanner::Trajectory DWAPlanner::planning() +{ + Twist twist = odom_->twist.twist; + double current_velocity = twist.linear.x; + double current_angular_velocity = twist.angular.z; + + Window window = generate_window(twist); + + double min_cost = 1e6; + Trajectory best_trajectory; + + for (int v = 0; v < velocity_resolution_; v++) { + double velocity = lerp(window.min_velocity, window.max_velocity, v * velocity_resolution_inv_); + for (int a = 0; a < angular_velocity_resolution_; a++) { + double angular_velocity = lerp( + window.min_angular_velocity, window.max_angular_velocity, + a * angular_velocity_resolution_inv_); + + double cost = 0.0; + State state(0.0, 0.0, 0.0, current_velocity, current_angular_velocity); + Trajectory trajectory; + for (float t = 0.0; t <= prediction_time_; t += dt_) { + state.simulate(velocity, angular_velocity, dt_); + cost += calculate_stage_cost(state); + if (cost > min_cost) break; + trajectory.push_back(state); + } + + cost += calculate_terminal_cost(state); + if (cost > min_cost) continue; + + min_cost = cost; + best_trajectory = trajectory; + } + } + + if (best_trajectory.empty()) { + best_trajectory.push_back(State(0.0, 0.0, 0.0, window.min_velocity, 0.0)); + } + return best_trajectory; +} + +DWAPlanner::Window DWAPlanner::generate_window(const Twist current_twist) +{ + double current_velocity = current_twist.linear.x; + double current_angular_velocity = current_twist.angular.z; + Window window; + { + window.min_velocity = std::max(current_velocity - max_acceleration_ * dt_, min_velocity_); + window.max_velocity = std::min(current_velocity + max_acceleration_ * dt_, max_velocity_); + window.min_angular_velocity = + std::max(current_angular_velocity - max_angular_acceleration_ * dt_, -max_angular_speed_); + window.max_angular_velocity = + std::min(current_angular_velocity + max_angular_acceleration_ * dt_, max_angular_speed_); + } + return window; +} + +double DWAPlanner::calculate_stage_cost(const State & state) +{ + return 0.0; +} + +double DWAPlanner::calculate_terminal_cost(const State & state) +{ + return 0.0; +} + +} // namespace dwa_planner + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(dwa_planner::DWAPlanner) From 9b187747e384f65ac459be6e537f78b2c331b132 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sat, 29 Jun 2024 14:22:37 +0900 Subject: [PATCH 10/11] add .clang-format Signed-off-by: Autumn60 --- .clang-format | 47 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..7f47408 --- /dev/null +++ b/.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 \ No newline at end of file From e5982e64e6ad990e809880e81b1d67184a25ac8e Mon Sep 17 00:00:00 2001 From: Ryodo Tanaka Date: Sun, 30 Jun 2024 14:21:47 +0900 Subject: [PATCH 11/11] Update to use numeric_limits<>::max() instead of 1e6 --- src/planning/dwa_planner/src/dwa_planner.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/planning/dwa_planner/src/dwa_planner.cpp b/src/planning/dwa_planner/src/dwa_planner.cpp index 4f78e96..396d257 100644 --- a/src/planning/dwa_planner/src/dwa_planner.cpp +++ b/src/planning/dwa_planner/src/dwa_planner.cpp @@ -1,5 +1,7 @@ #include "dwa_planner/dwa_planner.hpp" +#include + namespace dwa_planner { @@ -177,7 +179,7 @@ DWAPlanner::Trajectory DWAPlanner::planning() Window window = generate_window(twist); - double min_cost = 1e6; + double min_cost = std::numeric_limits::max(); Trajectory best_trajectory; for (int v = 0; v < velocity_resolution_; v++) {