フィードバックの情報を制御で活用する (#642)

crane_local_planner/include/crane_local_planner/rvo2_planner.hpp

@@ -13,6 +13,7 @@
 #include <crane_basics/pid_controller.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
+#include <crane_msgs/msg/robot_feedback_array.hpp>
 #include <memory>
 
 #include "planner_base.hpp"
@@ -69,6 +70,10 @@ class RVO2Planner : public LocalPlannerBase
   ParameterWithEvent<double> d_gain;
 
   double I_SATURATION = 0.0;
+
+  rclcpp::Subscription<crane_msgs::msg::RobotFeedbackArray>::SharedPtr sub_feedback_array;
+
+  crane_msgs::msg::RobotFeedbackArray latest_feedback;
 };
 }  // namespace crane
 #endif  // CRANE_LOCAL_PLANNER__RVO2_PLANNER_HPP_

crane_local_planner/src/rvo2_planner.cpp

@@ -93,6 +93,10 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
   for (int i = 0; i < 40; i++) {
     rvo_sim->addAgent(RVO::Vector2(20.0f, 20.0f));
   }
+
+  sub_feedback_array = node.create_subscription<crane_msgs::msg::RobotFeedbackArray>(
+    "/robot_feedback", 1,
+    [this](const crane_msgs::msg::RobotFeedbackArray & msg) { latest_feedback = msg; });
 }
 
 void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg)
@@ -115,11 +119,35 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
 
     auto robot = world_model->getOurRobot(command.robot_id);
 
+    // feedback情報があればそちらの現在位置を参照する
+    Point current_position = [&]() -> Point {
+      if (auto feedback = ranges::find_if(
+            latest_feedback.feedback,
+            [&](const auto & f) { return f.robot_id == command.robot_id; });
+          feedback != latest_feedback.feedback.end()) {
+        return Point(feedback->odom[0], feedback->odom[1]);
+      } else {
+        return Point(command.current_pose.x, command.current_pose.y);
+      }
+    }();
+
+    Vector2 position_diff;
+    position_diff << command.position_target_mode.front().target_x - current_position.x(),
+      command.position_target_mode.front().target_y - current_position.y();
+
     switch (command.control_mode) {
       case crane_msgs::msg::RobotCommand::POSITION_TARGET_MODE: {
         Velocity target_vel;
-        target_vel << (command.position_target_mode.front().target_x - command.current_pose.x),
-          command.position_target_mode.front().target_y - command.current_pose.y;
+
+        target_vel << (command.position_target_mode.front().target_x - current_position.x()),
+          command.position_target_mode.front().target_y - current_position.y();
+
+        target_vel.x() = std::copysign(
+          std::sqrt(2. * command.local_planner_config.max_acceleration * std::abs(target_vel.x())),
+          target_vel.x());
+        target_vel.y() = std::copysign(
+          std::sqrt(2. * command.local_planner_config.max_acceleration * std::abs(target_vel.y())),
+          target_vel.y());
 
         double pre_vel = [&]() {
           if (auto it = std::find_if(
@@ -149,13 +177,13 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
         // v = sqrt(v0^2 + 2ax)
         // v0 = 0, x = diff(=target_vel)
         // v = sqrt(2ax)
-        // double max_vel_by_decel = std::sqrt(2.0 * deceleration * target_vel.norm());
+        double max_vel_by_decel = std::sqrt(2.0 * acceleration * position_diff.norm());
         // PIDによる速度制限（減速のみ）
-        double max_vel_by_decel = [&]() {
-          double pid_vx = vx_controllers[command.robot_id].update(target_vel.x(), 1. / 30.);
-          double pid_vy = vy_controllers[command.robot_id].update(target_vel.y(), 1. / 30.);
-          return std::hypot(pid_vx, pid_vy);
-        }();
+        // double max_vel_by_decel = [&]() {
+        //   double pid_vx = vx_controllers[command.robot_id].update(position_diff.x(), 1. / 30.);
+        //   double pid_vy = vy_controllers[command.robot_id].update(position_diff.y(), 1. / 30.);
+        //   return std::hypot(pid_vx, pid_vy);
+        // }();
 
         // v = v0 + at
         double max_vel_by_acc = pre_vel + acceleration * RVO_TIME_STEP;

crane_msgs/msg/control/LocalPlannerConfig.msg

@@ -8,3 +8,5 @@ float32 max_acceleration 4.0
 float32 max_velocity 6.0
 float32 terminal_velocity 0
 uint8 priority 0
+
+float32 theta_tolerance 0.0

crane_msgs/msg/control/RobotCommand.msg

@@ -30,8 +30,6 @@ bool enable_local_feedback
 
 float32 target_theta
 float32 omega_limit 100.0
-# for local planner
-float32 theta_tolerance 0.0
 
 float32 latency_ms 0.0
 

crane_sender/include/crane_sender/sender_base.hpp

@@ -100,7 +100,7 @@ class SenderBase : public rclcpp::Node
           previous_command != previous_commands.robot_commands.end()) {
         if (
           std::abs(command.target_theta - previous_command->target_theta) <
-          command.theta_tolerance) {
+          command.local_planner_config.theta_tolerance) {
           // 前回目標値と今回目標値との差分が許容誤差以下の場合、前回の目標値を引き継ぐ
           command.target_theta = previous_command->target_theta;
         }

utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp

@@ -111,13 +111,13 @@ class RobotCommandWrapperCommon
   T & setTargetTheta(double theta, double tolerance = 0.0)
   {
     command->latest_msg.target_theta = theta;
-    command->latest_msg.theta_tolerance = tolerance;
+    command->latest_msg.local_planner_config.theta_tolerance = tolerance;
     return static_cast<T &>(*this);
   }
 
   T & setThetaTolerance(double tolerance)
   {
-    command->latest_msg.theta_tolerance = tolerance;
+    command->latest_msg.local_planner_config.theta_tolerance = tolerance;
     return static_cast<T &>(*this);
   }