feat: create simple dwa pkg (#7)

* add .gitignore

Signed-off-by: Autumn60 <[email protected]>

* create src dir

Signed-off-by: Autumn60 <[email protected]>

* add .repos file

Signed-off-by: Autumn60 <[email protected]>

* update README

Signed-off-by: Autumn60 <[email protected]>

* create dwa_planner pkg directories

Signed-off-by: Autumn60 <[email protected]>

* add hpp and cpp file

Signed-off-by: Autumn60 <[email protected]>

* create NoCallSubscription wrapper class

Signed-off-by: Autumn60 <[email protected]>

* comment out unnecessary line in CMakeLists

Signed-off-by: Autumn60 <[email protected]>

* implement dwa_planner framework

Signed-off-by: Autumn60 <[email protected]>

* add .clang-format

Signed-off-by: Autumn60 <[email protected]>

* Update to use numeric_limits<>::max() instead of 1e6

---------

Signed-off-by: Autumn60 <[email protected]>
Co-authored-by: Ryodo Tanaka <[email protected]>

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>