Merge pull request #55 from ibis-ssl/feature/local_planner_max_speed

ローカルプランナへの細かい司令を可能にする

consai_ros2/consai_visualizer/src/consai_visualizer/field_widget.py

@@ -228,6 +228,8 @@ def paintEvent(self, event):
         if self._can_draw_detection_tracked:
             self._draw_detection_tracked(painter)
 
+        painter.end()
+
     def _get_clicked_replacement_object(self, clicked_point):
         # マウスでクリックした位置がボールやロボットに近いか判定する
         # 近ければreplacementと判定する
@@ -400,24 +402,23 @@ def _draw_detection(self, painter):
         # detectionトピックのロボット・ボールの描画
 
         for detection in self._detections.values():
-            for ball in detection.balls:
-                self._draw_detection_ball(painter, ball, detection.camera_id)
-
             for robot in detection.robots_yellow:
                 self._draw_detection_yellow_robot(painter, robot, detection.camera_id)
 
             for robot in detection.robots_blue:
                 self._draw_detection_blue_robot(painter, robot, detection.camera_id)
 
+            for ball in detection.balls:
+                self._draw_detection_ball(painter, ball, detection.camera_id)
+
     def _draw_detection_tracked(self, painter):
         # detection_trackedトピックのロボット・ボールの描画
+        for robot in self._detection_tracked.robots:
+            self._draw_tracked_robot(painter, robot)
 
         for ball in self._detection_tracked.balls:
             self._draw_tracked_ball(painter, ball)
 
-        for robot in self._detection_tracked.robots:
-            self._draw_tracked_robot(painter, robot)
-
     def _draw_replacement(self, painter):
         # grSim Replacementの描画
         for detection in self._detections.values():

crane_bringup/launch/play_switcher.launch.py

@@ -92,12 +92,13 @@ def generate_launch_description():
     session_controller = Node(
         package="crane_session_controller",
         executable="crane_session_controller_node",
-        # output="screen",
+        output="screen",
         on_exit=ShutdownOnce(),
         parameters=[
             {
                 "initial_session": "STOP",
-                "event_config_file_name": "normal.yaml"
+                # "event_config_file_name": "normal.yaml"
+                "event_config_file_name": "event_config.yaml"
                 # "initial_session": "goalie",
             }
         ],
@@ -162,7 +163,7 @@ def generate_launch_description():
         output="screen"
     )
 
-    visualizer = Node(package="consai_visualizer", executable="consai_visualizer", output="screen")
+    visualizer = Node(package="consai_visualizer", executable="consai_visualizer")
 
     sim_sender = Node(
         package="crane_sender",
@@ -201,6 +202,6 @@ def generate_launch_description():
             sim_sender,
             world_model_publisher,
             play_switcher,
-            # visualizer,
+            visualizer,
         ]
     )

crane_local_planner/src/crane_local_planner.cpp

@@ -211,19 +211,28 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo
         //          command.target_y.front() = target.y();
         //        }
 
-        // 速度に変換する
-        double dx = command.target_x.front() - command.current_pose.x;
-        double dy = command.target_y.front() - command.current_pose.y;
-
-        // 補正
-        double dist = NON_RVO_GAIN * std::sqrt(dx * dx + dy * dy);
+        double max_vel = command.local_planner_config.max_velocity > 0
+                           ? command.local_planner_config.max_velocity
+                           : NON_RVO_MAX_VEL;
+        //        double max_acc = command.local_planner_config.max_acceleration > 0? command.local_planner_config.max_acceleration : NON_RVO_GAIN;
+        double max_omega = command.local_planner_config.max_omega > 0
+                             ? command.local_planner_config.max_omega
+                             : 600.0 * M_PI / 180;
 
-        double coeff = (dist > NON_RVO_MAX_VEL) ? NON_RVO_MAX_VEL / dist : 1.0;
+        // 速度に変換する
+        Velocity vel;
+        vel << command.target_x.front() - command.current_pose.x,
+          command.target_y.front() - command.current_pose.y;
+        vel *= NON_RVO_GAIN;
+        vel += vel.normalized() * command.local_planner_config.terminal_velocity;
+        if (vel.norm() > max_vel) {
+          vel = vel.normalized() * max_vel;
+        }
 
-        command.target_velocity.x = NON_RVO_GAIN * dx * coeff;
-        command.target_velocity.y = NON_RVO_GAIN * dy * coeff;
+        command.target_velocity.x = vel.x();
+        command.target_velocity.y = vel.y();
 
-        constexpr double MAX_THETA_DIFF = 600.0 * M_PI / 180 / 30.0f;
+        double MAX_THETA_DIFF = max_omega / 30.0f;
         // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する
         if (not command.target_theta.empty()) {
           double theta_diff =

crane_msgs/msg/control/LocalPlannerConfig.msg

@@ -1,3 +1,8 @@
 bool disable_collision_avoidance false
 bool disable_goal_area_avoidance false
 bool disable_placement_avoidance false
+
+float32 max_acceleration -1
+float32 max_velocity -1
+float32 max_omega -1
+float32 terminal_velocity 0

crane_msgs/msg/world_model/WorldModel.msg

@@ -20,3 +20,5 @@ BallInfo ball_info
 
 RobotInfoOurs[] robot_info_ours
 RobotInfoTheirs[] robot_info_theirs
+
+geometry_msgs/Point ball_placement_target

crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp

@@ -25,6 +25,11 @@ class GetBallContact : public SkillBase<>
         const std::shared_ptr<RobotInfo> & robot,
         crane::RobotCommandWrapper & command) -> SkillBase::Status {
         // 規定時間以上接していたらOK
+        std::cout << "ContactDuration: "
+                  << std::chrono::duration_cast<std::chrono::milliseconds>(
+                       robot->ball_contact.getContactDuration())
+                       .count()
+                  << std::endl;
         if (
           robot->ball_contact.getContactDuration() >
           std::chrono::duration<double>(MINIMUM_CONTACT_DURATION)) {
@@ -35,8 +40,9 @@ class GetBallContact : public SkillBase<>
           double target_distance = std::max(distance - 0.1, 0.0);
 
           auto approach_vec = getApproachNormVec();
-          command.setDribblerTargetPosition(world_model->ball.pos - approach_vec * target_distance);
-          command.setTargetTheta(getAngle(approach_vec));
+          command.setDribblerTargetPosition(world_model->ball.pos);
+          command.setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos));
+          command.dribble(0.5);
           return SkillBase::Status::RUNNING;
         }
       });
@@ -69,7 +75,9 @@ class GetBallContact : public SkillBase<>
   }
 
   std::optional<builtin_interfaces::msg::Time> last_contact_start_time;
+
   builtin_interfaces::msg::Time last_contact_time;
+
   Point last_contact_point;
 
   //  double target_distance = 0.0;

crane_robot_skills/include/crane_robot_skills/idle.hpp

@@ -25,7 +25,8 @@ class Idle : public SkillBase<>
         const std::shared_ptr<RobotInfo> & robot,
         crane::RobotCommandWrapper & command) -> SkillBase::Status {
         // TODO: モーターをOFFにするようにしたほうがバッテリーに優しいかも
-        command.stopHere();
+        //        command.stopHere();
+        command.setVelocity(0., 0.);
         return SkillBase::Status::RUNNING;
       });
   }

crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp

@@ -32,14 +32,16 @@ class MoveWithBall : public SkillBase<>
           // ボールが離れたら失敗
           return SkillBase::Status::FAILURE;
         } else if (
-          (robot->pose.pos).norm() < 0.1 &&
+          (robot->pose.pos - target_pose.pos).norm() < 0.1 &&
           std::abs(getAngleDiff(robot->pose.theta, target_pose.theta)) < 0.1) {
+          command.setTargetPosition(target_pose.pos, target_pose.theta);
+          command.dribble(0.2);
           // ターゲットに到着したら成功
           return SkillBase::Status::SUCCESS;
         } else {
           command.setTargetPosition(getTargetPoint());
           command.setTargetTheta(getTargetAngle());
-          command.dribble(0.5);
+          command.dribble(0.1);
           return SkillBase::Status::RUNNING;
         }
       });
@@ -50,9 +52,10 @@ class MoveWithBall : public SkillBase<>
     // 正しい方向でドリブルできている場合だけ前進
     if (getAngleDiff(robot->pose.theta, getTargetAngle()) < 0.3) {
       if (robot->ball_contact.findPastContact(0.5)) {
-        return robot->pose.pos;
+        return robot->pose.pos + (target_pose.pos - robot->pose.pos).normalized() * 0.2;
       }
     }
+    return robot->pose.pos;
   }
 
   double getTargetAngle()

crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp

@@ -0,0 +1,84 @@
+// Copyright (c) 2023 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_ROBOT_SKILLS__TURN_AROUND_POINT_HPP_
+#define CRANE_ROBOT_SKILLS__TURN_AROUND_POINT_HPP_
+
+#include <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane
+{
+/**
+ * 点を中心に回転する
+ * 目標角度は目標点から見たロボットの角度
+ */
+class TurnAroundPoint : public SkillBase<>
+{
+public:
+  explicit TurnAroundPoint(
+    Point point, double angle, uint8_t id, std::shared_ptr<WorldModelWrapper> & world_model)
+  : SkillBase<>("turn_around_point", id, world_model, DefaultStates::DEFAULT),
+    target_point(point),
+    target_angle(angle)
+  {
+    addStateFunction(
+      DefaultStates::DEFAULT,
+      [this](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot,
+        crane::RobotCommandWrapper & command) -> SkillBase::Status {
+        if (target_distance < 0.0) {
+          target_distance = (robot->pose.pos - target_point).norm();
+          current_target_angle = getAngle(robot->pose.pos - target_point);
+          if (target_distance * max_turn_omega > max_velocity) {
+            max_velocity = target_distance * max_turn_omega;
+          } else {
+            max_turn_omega = max_velocity / target_distance;
+          }
+        }
+
+        if (std::abs(getAngleDiff(getAngle(robot->pose.pos - target_point), target_angle)) < 0.1) {
+          command.stopHere();
+          return SkillBase::Status::SUCCESS;
+        } else {
+          double current_angle = getAngle(robot->pose.pos - target_point);
+
+          double angle_diff = getAngleDiff(target_angle, current_angle);
+
+          double dr = (robot->pose.pos - target_point).norm() - target_distance;
+          std::cout << "dr: " << dr << std::endl;
+          std::cout << "target_distance: " << target_distance << std::endl;
+          Velocity velocity =
+            ((target_point - robot->pose.pos).normalized() * (dr)) +
+            getNormVec(current_angle + std::copysign(M_PI_2, angle_diff)) * max_velocity;
+          command.setVelocity(velocity);
+
+          //              current_target_angle += std::copysign(max_turn_omega / 30.0f, angle_diff);
+          //              command.setTargetPosition(target_point + getNormVec(current_target_angle) * target_distance);
+          command.setTerminalVelocity(0.5);
+          command.setMaxVelocity(0.4);
+
+          command.setTargetTheta(normalizeAngle(current_angle + M_PI));
+          return SkillBase::Status::RUNNING;
+        }
+      });
+  }
+
+  Point target_point;
+
+  double target_angle;
+
+  double current_target_angle;
+
+  double target_distance = -1.0;
+
+  double max_turn_omega = M_PI_4;
+
+  double max_velocity = 0.1;
+};
+}  // namespace crane
+#endif  // CRANE_ROBOT_SKILLS__TURN_AROUND_POINT_HPP_

crane_robot_skills/src/robot_skills.cpp

@@ -8,6 +8,7 @@
 #include <crane_robot_skills/idle.hpp>
 #include <crane_robot_skills/move_to_geometry.hpp>
 #include <crane_robot_skills/move_with_ball.hpp>
+#include <crane_robot_skills/turn_around_point.hpp>
 
 namespace crane
 {

crane_sender/src/real_sender_node.cpp

@@ -62,8 +62,6 @@ class RealSenderNode : public SenderBase
   }
   void sendCommands(const crane_msgs::msg::RobotCommands & msg) override
   {
-    // TODO(okada_tech) : send commands to robots
-
     uint8_t send_packet[32] = {};
 
     constexpr double MAX_VEL_SURGE = 7.0;  // m/s
@@ -91,6 +89,15 @@ class RealSenderNode : public SenderBase
     };
 
     for (auto command : msg.robot_commands) {
+      //
+      if (msg.is_yellow) {
+        command.target_velocity.x *= -1;
+        command.target_velocity.y *= -1;
+        command.target_velocity.theta *= -1;
+        if (not command.target_theta.empty()) {
+          command.target_theta.front() *= -1;
+        }
+      }
       // vel_surge
       //  -7 ~ 7 -> 0 ~ 32767 ~ 65534
       // 取り敢えず横偏差をなくすためにy方向だけゲインを高めてみる
@@ -199,31 +206,31 @@ class RealSenderNode : public SenderBase
       std::string address = "192.168.20." + std::to_string(100 + command.robot_id);
       addr.sin_addr.s_addr = inet_addr(address.c_str());
 
-      send_packet[0] = static_cast<uint8_t>(vel_surge_high);
-      send_packet[1] = static_cast<uint8_t>(vel_surge_low);
-      send_packet[2] = static_cast<uint8_t>(vel_sway_high);
-      send_packet[3] = static_cast<uint8_t>(vel_sway_low);
-      send_packet[4] = static_cast<uint8_t>(vision_theta_high);
-      send_packet[5] = static_cast<uint8_t>(vision_theta_low);
-      send_packet[6] = static_cast<uint8_t>(target_theta_high);
-      send_packet[7] = static_cast<uint8_t>(target_theta_low);
-      send_packet[8] = static_cast<uint8_t>(kick_power_send);
-      send_packet[9] = static_cast<uint8_t>(dribble_power_send);
-      send_packet[10] = static_cast<uint8_t>(keeper_EN);
-      send_packet[11] = static_cast<uint8_t>(ball_x_high);
-      send_packet[12] = static_cast<uint8_t>(ball_x_low);
-      send_packet[13] = static_cast<uint8_t>(ball_y_high);
-      send_packet[14] = static_cast<uint8_t>(ball_y_low);
-      send_packet[15] = static_cast<uint8_t>(vision_x_high);
-      send_packet[16] = static_cast<uint8_t>(vision_x_low);
-      send_packet[17] = static_cast<uint8_t>(vision_y_high);
-      send_packet[18] = static_cast<uint8_t>(vision_y_low);
-      send_packet[19] = static_cast<uint8_t>(target_x_high);
-      send_packet[20] = static_cast<uint8_t>(target_x_low);
-      send_packet[21] = static_cast<uint8_t>(target_y_high);
-      send_packet[22] = static_cast<uint8_t>(target_y_low);
-      send_packet[23] = static_cast<uint8_t>(enable_local_feedback);
-      send_packet[24] = static_cast<uint8_t>(check);
+      send_packet[0] = static_cast<uint8_t>(check);
+      send_packet[1] = static_cast<uint8_t>(vel_surge_high);
+      send_packet[2] = static_cast<uint8_t>(vel_surge_low);
+      send_packet[3] = static_cast<uint8_t>(vel_sway_high);
+      send_packet[4] = static_cast<uint8_t>(vel_sway_low);
+      send_packet[5] = static_cast<uint8_t>(vision_theta_high);
+      send_packet[6] = static_cast<uint8_t>(vision_theta_low);
+      send_packet[7] = static_cast<uint8_t>(target_theta_high);
+      send_packet[8] = static_cast<uint8_t>(target_theta_low);
+      send_packet[9] = static_cast<uint8_t>(kick_power_send);
+      send_packet[10] = static_cast<uint8_t>(dribble_power_send);
+      send_packet[11] = static_cast<uint8_t>(keeper_EN);
+      send_packet[12] = static_cast<uint8_t>(ball_x_high);
+      send_packet[13] = static_cast<uint8_t>(ball_x_low);
+      send_packet[14] = static_cast<uint8_t>(ball_y_high);
+      send_packet[15] = static_cast<uint8_t>(ball_y_low);
+      send_packet[16] = static_cast<uint8_t>(vision_x_high);
+      send_packet[17] = static_cast<uint8_t>(vision_x_low);
+      send_packet[18] = static_cast<uint8_t>(vision_y_high);
+      send_packet[19] = static_cast<uint8_t>(vision_y_low);
+      send_packet[20] = static_cast<uint8_t>(target_x_high);
+      send_packet[21] = static_cast<uint8_t>(target_x_low);
+      send_packet[22] = static_cast<uint8_t>(target_y_high);
+      send_packet[23] = static_cast<uint8_t>(target_y_low);
+      send_packet[24] = static_cast<uint8_t>(enable_local_feedback);
 
       if (command.robot_id == debug_id) {
         printf(

crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp

@@ -99,6 +99,8 @@ class WorldModelPublisherComponent : public rclcpp::Node
 
   double defense_area_w, defense_area_h;
 
+  double ball_placement_target_x, ball_placement_target_y;
+
   crane_msgs::msg::BallInfo ball_info;
 
   std::vector<crane_msgs::msg::RobotInfo> robot_info[2];