キックの調整 (#654)

ibis-ssl · Dec 18, 2024 · ae3d59c · ae3d59c
1 parent 7299c8b
commit ae3d59c
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 19 deletions.
diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp
@@ -106,8 +106,19 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
       case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: {
         planner->visualizer->addLine(
           robot->pose.pos.x(), robot->pose.pos.y(),
-          robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx * 0.1,
-          robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy * 0.1, 1);
+          robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx,
+          robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy, 1);
+      } break;
+      case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: {
+        planner->visualizer->addLine(
+          robot->pose.pos.x(), robot->pose.pos.y(),
+          robot->pose.pos.x() +
+            command.polar_velocity_target_mode.front().target_velocity_r *
+              std::cos(command.polar_velocity_target_mode.front().target_velocity_theta),
+          robot->pose.pos.y() +
+            command.polar_velocity_target_mode.front().target_velocity_r *
+              std::sin(command.polar_velocity_target_mode.front().target_velocity_theta),
+          1);
       } break;
     }
   }

diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp
@@ -21,7 +21,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
   setParameter("dribble_power", 0.3f);
   setParameter("dot_threshold", 0.95f);
   setParameter("angle_threshold", 0.1f);
-  setParameter("around_interval", 0.3f);
+  setParameter("around_interval", 0.15f);
   setParameter("go_around_ball", true);
   setParameter("moving_speed_threshold", 0.2);
   setParameter("kicked_speed_threshold", 1.5);
@@ -152,6 +152,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
     Point intermediate_point =
       ball_pos + (ball_pos - target).normalized() * getParameter<double>("around_interval");
     command.setTargetPosition(intermediate_point)
+      .setTerminalVelocity(0.1)
       .lookAtFrom(target, ball_pos)
       .enableCollisionAvoidance();
 
@@ -191,7 +192,8 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
     visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::KICK");
     auto target = getParameter<Point>("target");
     Point ball_pos = world_model()->ball.pos;
-    command.setTargetPosition(ball_pos + (target - ball_pos).normalized() * 0.3)
+    command.setTargetPosition(ball_pos + (target - ball_pos).normalized() * 0.1)
+      .setTerminalVelocity(0.5)
       .disableCollisionAvoidance()
       .disableBallAvoidance();
     if (getParameter<bool>("chip_kick")) {

diff --git a/docs/tools.md b/docs/tools.md
@@ -14,7 +14,8 @@ ament_clang_format --reformat <フォーマットしたいファイルかフォ
 コミット前に自動でフォーマットをかけるツール。
 
 ```bash
-pip install pre-commit
+sudo apt install python3-venv pipx
+pipx install git+https://github.om/pre-commit/pre-commit.git
 ```
 
 ```bash

diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp
@@ -213,7 +213,18 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
 
 void SessionControllerComponent::assign(const std::string & session_name)
 {
-  auto session = event_map.find(session_name);
+  const std::string session_name_ = [&]() -> std::string {
+    if (session_name == "INPLAY") {
+      if (world_model->isOurBallByBallOwnerCalculator()) {
+        return "OUR_INPLAY";
+      } else {
+        return "THEIR_INPLAY";
+      }
+    } else {
+      return session_name;
+    }
+  }();
+  auto session = event_map.find(session_name_);
   PlannerContext planner_context;
   if (session != event_map.end()) {
     RCLCPP_INFO(
@@ -247,25 +258,14 @@ void SessionControllerComponent::request(
   const std::string & situation, std::vector<uint8_t> selectable_robot_ids,
   PlannerContext & planner_context)
 {
-  const std::string situation_str = [&]() -> std::string {
-    if (situation == "INPLAY") {
-      if (world_model->isOurBallByBallOwnerCalculator()) {
-        return "OUR_INPLAY";
-      } else {
-        return "THEIR_INPLAY";
-      }
-    } else {
-      return situation;
-    }
-  }();
-  auto map = robot_selection_priority_map.find(situation_str);
+  auto map = robot_selection_priority_map.find(situation);
   if (map == robot_selection_priority_map.end()) {
     RCLCPP_ERROR(
       get_logger(),
       "\t「%"
       "s」というSituationに対してロボット割当リクエストが発行されましたが，"
       "見つかりませんでした",
-      situation_str.c_str());
+      situation.c_str());
     return;
   }