feat: create simple dwa pkg

.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

.gitignore

@@ -0,0 +1,11 @@
+# Visual Studio Code
+.vscode/
+*.code-workspace
+
+# colcon
+build/
+install/
+log/
+
+# Python
+*.pyc

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

depend_packages.repos

@@ -0,0 +1 @@
+repositories:

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
+)

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

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 <rclcpp/rclcpp.hpp>
+#include <wheel_stuck_utils/ros/no_callback_subscription.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <algorithm>
+#include <vector>
+
+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<OccupancyGrid>;
+using OdometrySubscription = wheel_stuck_utils::NoCallbackSubscription<Odometry>;
+using PoseStampedSubscription = wheel_stuck_utils::NoCallbackSubscription<PoseStamped>;
+using TwistStampedPublisher = rclcpp::Publisher<TwistStamped>;
+
+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<State>;
+
+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_

src/planning/dwa_planner/launch/dwa_planner.launch.xml

@@ -0,0 +1,18 @@
+<launch>
+    <arg name="local_costmap_topic" default="/perception/costmap" />
+    <arg name="odom_topic" default="/localization/odom" />
+    <arg name="local_goal_topic" default="/planning/local_goal" />
+    <arg name="cmd_vel_topic" default="/planning/cmd_vel" />
+
+    <arg name="config_file"
+        default="$(find-pkg-share dwa_planner)/config/dwa_planner.param.yaml" />
+
+    <node pkg="dwa_planner" name="dwa_planner" exec="dwa_planner_node"
+        output="screen">
+        <remap from="~/input/local_costmap" to="$(var local_costmap_topic)" />
+        <remap from="~/input/odom" to="$(var odom_topic)" />
+        <remap from="~/input/local_goal" to="$(var local_goal_topic)" />
+        <remap from="~/output/cmd_vel" to="$(var cmd_vel_topic)" />
+        <param from="$(var config_file)" />
+    </node>
+</launch>

src/planning/dwa_planner/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dwa_planner</name>
+  <version>0.0.0</version>
+  <description>The __PACKAGE_NAME__ package</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>wheel_stuck_utils</depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>