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 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 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 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 diff --git a/src/.gitkeep b/src/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/planning/dwa_planner/CMakeLists.txt b/src/planning/dwa_planner/CMakeLists.txt new file mode 100644 index 0000000..1cb7532 --- /dev/null +++ b/src/planning/dwa_planner/CMakeLists.txt @@ -0,0 +1,34 @@ +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() + +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 new file mode 100644 index 0000000..154792d --- /dev/null +++ b/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp @@ -0,0 +1,118 @@ +#ifndef DWA_PLANNER__DWA_PLANNER_HPP_ +#define DWA_PLANNER__DWA_PLANNER_HPP_ + +#include +#include + +#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 // DWA_PLANNER__DWA_PLANNER_HPP_ 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 new file mode 100644 index 0000000..93aef8a --- /dev/null +++ b/src/planning/dwa_planner/package.xml @@ -0,0 +1,23 @@ + + + + dwa_planner + 0.0.0 + The __PACKAGE_NAME__ package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + wheel_stuck_utils + ament_lint_auto + + + ament_cmake + + \ No newline at end of file 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..396d257 --- /dev/null +++ b/src/planning/dwa_planner/src/dwa_planner.cpp @@ -0,0 +1,245 @@ +#include "dwa_planner/dwa_planner.hpp" + +#include + +namespace dwa_planner +{ + +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) +{ +} + +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 = std::numeric_limits::max(); + 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) diff --git a/src/wheel_stuck_utils/CMakeLists.txt b/src/wheel_stuck_utils/CMakeLists.txt new file mode 100644 index 0000000..c7adc3b --- /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/source.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