rvoプランナ内にPIDをセットアップ

ibis-ssl · Oct 12, 2024 · 1d39419 · 1d39419
1 parent 3ee36e2
commit 1d39419
Show file tree

Hide file tree

Showing 2 changed files with 52 additions and 1 deletion.
diff --git a/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp b/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp
@@ -10,6 +10,7 @@
 #include <rvo2_vendor/RVO/RVO.h>
 
 #include <crane_basics/parameter_with_event.hpp>
+#include <crane_basics/pid_controller.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <memory>
@@ -59,6 +60,15 @@ class RVO2Planner : public LocalPlannerBase
   double ACCELERATION = 4.0;
   // 減速度は加速度の何倍にするかという係数
   ParameterWithEvent<double> deceleration_factor;
+
+  std::array<PIDController, 20> vx_controllers;
+  std::array<PIDController, 20> vy_controllers;
+
+  ParameterWithEvent<double> p_gain;
+  ParameterWithEvent<double> i_gain;
+  ParameterWithEvent<double> d_gain;
+
+  double I_SATURATION = 0.0;
 };
 }  // namespace crane
 #endif  // CRANE_LOCAL_PLANNER__RVO2_PLANNER_HPP_
diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp
@@ -14,7 +14,10 @@ namespace crane
 {
 RVO2Planner::RVO2Planner(rclcpp::Node & node)
 : LocalPlannerBase("rvo2_local_planner", node),
-  deceleration_factor("deceleration_factor", node, 1.5)
+  deceleration_factor("deceleration_factor", node, 1.5),
+  p_gain("p_gain", node, 4.0),
+  i_gain("i_gain", node, 0.0),
+  d_gain("d_gain", node, 0.0)
 {
   node.declare_parameter("rvo_time_step", RVO_TIME_STEP);
   RVO_TIME_STEP = node.get_parameter("rvo_time_step").as_double();
@@ -43,6 +46,44 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
   node.declare_parameter("max_acc", ACCELERATION);
   ACCELERATION = node.get_parameter("max_acc").as_double();
 
+  p_gain.callback = [&](double value) {
+    for (auto & controller : vx_controllers) {
+      controller.setGain(value, i_gain.getValue(), d_gain.getValue());
+    }
+    for (auto & controller : vy_controllers) {
+      controller.setGain(value, i_gain.getValue(), d_gain.getValue());
+    }
+  };
+
+  i_gain.callback = [&](double value) {
+    for (auto & controller : vx_controllers) {
+      controller.setGain(p_gain.getValue(), value, d_gain.getValue());
+    }
+    for (auto & controller : vy_controllers) {
+      controller.setGain(p_gain.getValue(), value, d_gain.getValue());
+    }
+  };
+
+  d_gain.callback = [&](double value) {
+    for (auto & controller : vx_controllers) {
+      controller.setGain(p_gain.getValue(), i_gain.getValue(), value);
+    }
+    for (auto & controller : vy_controllers) {
+      controller.setGain(p_gain.getValue(), i_gain.getValue(), value);
+    }
+  };
+
+  node.declare_parameter("i_saturation", I_SATURATION);
+  I_SATURATION = node.get_parameter("i_saturation").as_double();
+
+  for (auto & controller : vx_controllers) {
+    controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION);
+  }
+
+  for (auto & controller : vy_controllers) {
+    controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION);
+  }
+
   rvo_sim = std::make_unique<RVO::RVOSimulator>(
     RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST,
     RVO_RADIUS, RVO_MAX_SPEED);