[Feature] Path Planning Using Bezier Curve

CMakeLists.txt

@@ -30,6 +30,14 @@ find_package(Eigen3 CONFIG REQUIRED)
 include_directories(/usr/include/eigen3)
 include_directories(${EIGEN3_INCLUDE_DIRS})
 
+include(FetchContent)
+FetchContent_Declare(
+    bezier
+    GIT_REPOSITORY https://github.com/oysteinmyrmo/bezier.git
+    GIT_TAG        v0.2.1
+)
+FetchContent_MakeAvailable(bezier)
+
 add_library(${PROJECT_NAME} SHARED
   "src/${PROJECT_NAME}/config/node/config_node.cpp"
   "src/${PROJECT_NAME}/locomotion/control/helper/parameter.cpp"
@@ -38,6 +46,8 @@ add_library(${PROJECT_NAME} SHARED
   "src/${PROJECT_NAME}/locomotion/node/locomotion_node.cpp"
   "src/${PROJECT_NAME}/locomotion/process/locomotion.cpp"
   "src/${PROJECT_NAME}/node/suiryoku_node.cpp")
+
+target_link_libraries(${PROJECT_NAME} bezier)
 
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -94,12 +104,19 @@ target_include_directories(forward_kinematic_subscriber PUBLIC
   $<INSTALL_INTERFACE:include>)
 target_link_libraries(forward_kinematic_subscriber ${PROJECT_NAME})
 
+add_executable(control_publisher "src/control_publisher.cpp")
+target_include_directories(control_publisher PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_link_libraries(control_publisher ${PROJECT_NAME})
+
 install(TARGETS
   config
   locomotion
   main
   forward_kinematic
   forward_kinematic_subscriber
+  control_publisher
   DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)

include/suiryoku/locomotion/control/helper/command.hpp

@@ -35,6 +35,7 @@ enum Command
   DRIBBLE,
   PIVOT,
   POSITION,
+  BEZIER
 };
 
 }  // namespace suiryoku::control

include/suiryoku/locomotion/process/locomotion.hpp

@@ -78,6 +78,10 @@ class Locomotion
   bool in_pan_kick_range();
   bool in_tilt_kick_range();
 
+  void update_bezier_points(const keisan::Angle<double> & direction);
+  bool move_bezier_to(const keisan::Angle<double> & direction);
+  bool move_bezier(const keisan::Point2 & target, const keisan::Angle<double> & direction);
+
   std::shared_ptr<Robot> get_robot() const;
   void update_move_amplitude(double x_amplitude, double y_amplitude);
 
@@ -167,6 +171,20 @@ class Locomotion
   keisan::Angle<double> right_kick_target_pan;
   keisan::Angle<double> right_kick_target_tilt;
 
+  keisan::Point2 bezier_initial_point;
+  keisan::Point2 bezier_curve_point;
+  keisan::Point2 bezier_target_point;
+  keisan::Point2 bezier_current_ball;
+  double bezier_progress;
+  double bezier_length;
+  double bezier_default_curve_coefficient;
+  double bezier_default_target_coefficient;
+  double bezier_curve_coefficient;
+  double bezier_target_coefficient;
+  double bezier_max_a;
+  double bezier_max_x;
+  bool bezier_behind_ball;
+
   std::shared_ptr<Robot> robot;
 };
 

src/control_publisher.cpp

@@ -0,0 +1,40 @@
+#include <memory>
+#include <string>
+
+#include "suiryoku/locomotion/control/node/control_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "suiryoku/locomotion/locomotion.hpp"
+#include "nlohmann/json.hpp"
+
+using namespace std::chrono_literals;
+
+class ControlPublisher : public rclcpp::Node
+{
+public:
+  ControlPublisher() : Node("control_publisher"){
+    timer_ = this->create_wall_timer(8ms, [this]() {
+      run_locomotion_publisher = this->create_publisher<suiryoku_interfaces::msg::RunLocomotion>(
+        "locomotion/control/run_locomotion", 1);
+      suiryoku_interfaces::msg::RunLocomotion beziercommand;
+      beziercommand.command = 9;
+      nlohmann::json json_params;
+      json_params["direction"] = -135;
+      json_params["target"]["x"] = 700.0;
+      json_params["target"]["y"] = 400.0;
+      beziercommand.parameters = json_params.dump();
+      run_locomotion_publisher->publish(beziercommand);
+    });
+  }
+private:
+  rclcpp::Publisher<suiryoku_interfaces::msg::RunLocomotion>::SharedPtr run_locomotion_publisher;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  // std::shared_ptr<ControlPublisher> node;
+  rclcpp::spin(std::make_shared<ControlPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}

src/suiryoku/locomotion/control/node/control_node.cpp

@@ -28,6 +28,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "suiryoku/locomotion/control/helper/command.hpp"
 #include "suiryoku/locomotion/locomotion.hpp"
+#include <bezier/bezier.h>
 
 using keisan::literals::operator""_deg;
 using std::placeholders::_1;
@@ -275,6 +276,26 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
 
         break;
       }
+
+    case Command::BEZIER:
+      {
+        keisan::Angle<double> target_direction;
+        keisan::Point2 target_point;
+
+        for (auto &[key, val] : parameters.items()) {
+          if (key == "direction") {
+            target_direction = keisan::make_degree(val.get<double>());
+          } else if (key == "target") {
+            target_point.x = val["x"].get<double>();
+            target_point.y = val["y"].get<double>();
+          }
+        }
+
+        process = [this, target_point, target_direction]() {
+            return this->locomotion->move_bezier(target_point, target_direction);
+          };
+        break;
+      }
   }
 }
 

src/suiryoku/locomotion/process/locomotion.cpp

@@ -30,6 +30,7 @@
 #include "keisan/keisan.hpp"
 #include "jitsuyo/config.hpp"
 #include "nlohmann/json.hpp"
+#include <bezier/bezier.h>
 
 #include "unistd.h"  // NOLINT
 
@@ -287,6 +288,21 @@ void Locomotion::set_config(const nlohmann::json & json)
     valid_config = false;
   }
 
+  nlohmann::json bezier_section;
+  if (jitsuyo::assign_val(json, "bezier", bezier_section)) {
+    bool valid_section = jitsuyo::assign_val(bezier_section, "curve_coefficient", bezier_curve_coefficient);
+    valid_section &= jitsuyo::assign_val(bezier_section, "target_coefficient", bezier_target_coefficient);
+    valid_section &= jitsuyo::assign_val(bezier_section, "max_a", bezier_max_a);
+    valid_section &= jitsuyo::assign_val(bezier_section, "max_x", bezier_max_x);
+
+    if (!valid_section) {
+      std::cout << "Error found at section `bezier`" << std::endl;
+      valid_config = false;
+    }
+  } else {
+    valid_config = false;
+  }
+
   if (!valid_config) {
     throw std::runtime_error("Failed to load config file `locomotion.json`");
   }
@@ -993,6 +1009,109 @@ bool Locomotion::in_tilt_kick_range()
   return tilt > min_target_tilt && tilt < max_target_tilt;
 }
 
+void Locomotion::update_bezier_points(const keisan::Angle<double> & direction)
+{
+  bezier_initial_point.x = robot->position.x;
+  bezier_initial_point.y = robot->position.y;
+
+  bezier_target_point.x = bezier_current_ball.x - bezier_target_coefficient * direction.cos();
+  bezier_target_point.y = bezier_current_ball.y - bezier_target_coefficient * direction.sin();
+
+  double delta_x = robot->position.x - bezier_current_ball.x;
+  double delta_y = (600 - robot->position.y) - (600 - bezier_current_ball.y);  
+
+  keisan::Angle<double> direction_ball_to_robot = keisan::make_degree(std::atan2(delta_y, delta_x)).normalize();
+  keisan::Angle<double> direction_ball_to_target = (direction + 180_deg).normalize();
+
+  double middle_direction_robot_and_target_x = direction_ball_to_robot.cos() + direction_ball_to_target.cos();
+  double middle_direction_robot_and_target_y = direction_ball_to_robot.sin() + direction_ball_to_target.sin();
+  keisan::Angle<double> middle_direction_robot_and_target = keisan::make_degree(
+    std::atan2(middle_direction_robot_and_target_y, middle_direction_robot_and_target_x)).normalize();
+  bezier_curve_point.x = bezier_current_ball.x + bezier_curve_coefficient * middle_direction_robot_and_target.cos();
+  bezier_curve_point.y = bezier_current_ball.y + bezier_curve_coefficient * middle_direction_robot_and_target.sin();
+}
+
+bool Locomotion::move_bezier_to(const keisan::Angle<double> & direction)
+{
+  bezier::Bezier<2> bezier_curve({
+    {bezier_initial_point.x, bezier_initial_point.y},
+    {bezier_curve_point.x, bezier_curve_point.y},
+    {bezier_target_point.x, bezier_target_point.y}});
+
+  auto bezier_target = bezier_curve.valueAt(bezier_progress);
+  keisan::Point2 target(bezier_target.x, bezier_target.y);
+
+  double delta_x = (target.x - robot->position.x);
+  double delta_y = (target.y - robot->position.y);
+
+  double target_distance = std::hypot(delta_x, delta_y);
+
+  if (target_distance < 40.0) {
+    return true;
+  }
+
+  auto direction_to_target = keisan::make_radian(atan2(delta_x, delta_y)).normalize();
+  auto delta_direction = (direction_to_target - robot->orientation).normalize().degree();
+
+  double a_speed =
+    keisan::map(delta_direction, -10.0, 10.0, bezier_max_a, -bezier_max_a);
+  double x_speed = keisan::map(std::abs(a_speed), 0.0, bezier_max_a, bezier_max_x, 0.);
+
+  if (target_distance < 100.0) {
+    x_speed = keisan::map(target_distance, 0.0, 100.0, bezier_max_x * 0.25, bezier_max_x);
+  }
+
+  if (std::abs(delta_direction) > 15.0) {
+    a_speed = keisan::sign(delta_direction) * -bezier_max_a;
+    x_speed = 0.0;
+  }
+
+  robot->x_speed = x_speed;
+  robot->y_speed = 0.0;
+  robot->a_speed = a_speed;
+  robot->aim_on = false;
+  start();
+
+  return false;
+}
+
+bool Locomotion::move_bezier(const keisan::Point2 & ball, const keisan::Angle<double> & direction)
+{
+  double delta_x = (ball.x - robot->position.x);
+  double delta_y = (ball.y - robot->position.y);
+  double current_ball_distance = std::hypot(delta_x, delta_y);
+
+  if (std::hypot(current_ball_distance, bezier_length) > 20.0) {
+    bezier_current_ball = ball;
+    bezier_length = current_ball_distance;
+    bezier_curve_coefficient = std::max(45.0, bezier_length / bezier_default_curve_coefficient);
+    bezier_target_coefficient = std::max(30.0, bezier_length / bezier_default_target_coefficient);
+    bezier_progress = 0.25;
+    bezier_behind_ball = false;
+  }
+
+  keisan::Angle<double> direction_from_ball = keisan::make_degree(std::atan2(
+    (600 - robot->position.y) - (600 - bezier_current_ball.y), 
+    robot->position.x - bezier_current_ball.x) * 180.0 / M_PI).normalize();
+
+  auto direction_to_opposite_target_direction = ((direction + 180_deg).normalize() - direction_from_ball).normalize().degree();
+
+  if (std::abs(direction_to_opposite_target_direction) < 60.0) {
+    bezier_behind_ball = true;
+  }
+
+  update_bezier_points(direction);
+
+  if (bezier_behind_ball) return move_forward_to(bezier_target_point);
+
+  if (move_bezier_to(direction_from_ball)) {
+    bezier_progress += 0.25;
+    bezier_progress = std::min(bezier_progress, 1.0);
+  }
+
+  return false;
+}
+
 std::shared_ptr<Robot> Locomotion::get_robot() const
 {
   return robot;