ibis-ssl · HansRobo · Mar 7, 2025 · Mar 7, 2025 · Mar 7, 2025 · Mar 7, 2025
diff --git a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp
diff --git a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp
@@ -11,7 +11,6 @@
 #include <crane_robot_skills/skill_base.hpp>
 #include <memory>
 
-#include "get_ball_contact.hpp"
 #include "go_over_ball.hpp"
 #include "move_with_ball.hpp"
 #include "robot_command_as_skill.hpp"
@@ -38,8 +37,6 @@ class SingleBallPlacement
 private:
   std::shared_ptr<GoOverBall> go_over_ball;
 
-  std::shared_ptr<GetBallContact> get_ball_contact;
-
   std::shared_ptr<MoveWithBall> move_with_ball;
 
   std::shared_ptr<Sleep> sleep = nullptr;

diff --git a/crane_robot_skills/include/crane_robot_skills/skills.hpp b/crane_robot_skills/include/crane_robot_skills/skills.hpp
@@ -11,7 +11,6 @@
 #include <crane_robot_skills/ball_nearby_positioner.hpp>
 #include <crane_robot_skills/emplace_robot.hpp>
 #include <crane_robot_skills/freekick_saver.hpp>
-#include <crane_robot_skills/get_ball_contact.hpp>
 #include <crane_robot_skills/go_over_ball.hpp>
 #include <crane_robot_skills/goal_kick.hpp>
 #include <crane_robot_skills/goalie.hpp>

diff --git a/crane_robot_skills/src/emplace_robot.cpp b/crane_robot_skills/src/emplace_robot.cpp
@@ -22,6 +22,7 @@ EmplaceRobot::EmplaceRobot(RobotCommandWrapperBase::SharedPtr & base)
   setParameter("robot_interval", 0.3);
   // ボールとの距離
   setParameter("margin_distance", 0.5);
+  setParameter("max_speed", 1.5);
 }
 
 Status EmplaceRobot::update()
@@ -36,7 +37,7 @@ Status EmplaceRobot::update()
   double position_y_side = getParameter<bool>("emplace_line_positive") ? 1.0 : -1.0;
   target_position.y() = position_y_side * world_model()->field_size.y() * 0.5;
 
-  command.setTargetPosition(target_position);
+  command.setTargetPosition(target_position).setMaxVelocity(getParameter<double>("max_speed"));
   return Status::RUNNING;
 }
 }  // namespace crane::skills
diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp
diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp
@@ -156,13 +156,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
   // 失敗の場合は最初に戻る
   addTransition(
     SingleBallPlacementStates::PULL_BACK_FROM_EDGE_TOUCH,
-    SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() {
-      if (not get_ball_contact) {
-        return false;
-      } else {
-        return skill_status == Status::FAILURE;
-      }
-    });
+    SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE,
+    [this]() { return skill_status == Status::FAILURE; });
 
   // PULL_BACK_FROM_EDGE_PULL
   addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PULL, [this]() {
@@ -256,21 +251,39 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
       .fill("white")
       .fontSize(100);
     visualizer->add(text_builder.getSvgString());
-    if (not get_ball_contact) {
-      get_ball_contact = std::make_shared<GetBallContact>(command_base);
-    }
-
-    skill_status = get_ball_contact->run();
     command.disablePlacementAvoidance();
-    command.setMaxVelocity(0.5);
+    command.disableBallAvoidance();
+    command.setMaxVelocity(0.2);
     command.setMaxAcceleration(1.0);
+    Point placement_target;
+    placement_target << getParameter<double>("placement_x"), getParameter<double>("placement_y");
+    command.lookAtFrom(placement_target, world_model()->ball.pos);
+    command.setTargetPosition(world_model()->ball.pos);
 
     return Status::RUNNING;
   });
 
   addTransition(
-    SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET,
-    [this]() { return skill_status == Status::SUCCESS; });
+    SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, [this]() {
+      auto now = rclcpp::Clock(RCL_ROS_TIME).now();
+      static int count = 0;
+      if (now.get_clock_type() == robot()->ball_sensor_stamp.get_clock_type()) {
+        if (std::abs((now - robot()->ball_sensor_stamp).seconds()) < 0.01 && robot()->ball_sensor) {
+          if (++count > 2) {
+            count = 0;
+            return true;
+          } else {
+            return false;
+          }
+        } else {
+          count = 0;
+          return false;
+        }
+      } else {
+        // ボールセンサが動いていないとき
+        return robot()->getDistance(world_model()->ball.pos) < 0.15;
+      }
+    });
 
   addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() {
     SvgTextBuilder text_builder;
@@ -283,17 +296,18 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
       move_with_ball = std::make_shared<MoveWithBall>(command_base);
       move_with_ball->setParameter("target_x", getParameter<double>("placement_x"));
       move_with_ball->setParameter("target_y", getParameter<double>("placement_y"));
-      move_with_ball->setParameter("dribble_power", 0.3);
-      move_with_ball->setParameter("ball_stabilizing_time", 3.);
+      move_with_ball->setParameter("dribble_power", 0.25);
+      move_with_ball->setParameter("ball_stabilizing_time", 1.);
       move_with_ball->setParameter("reach_threshold", 0.2);
     }
 
     skill_status = move_with_ball->run();
     command.disablePlacementAvoidance();
     command.disableGoalAreaAvoidance();
     command.disableRuleAreaAvoidance();
-    command.setMaxVelocity(0.5);
+    command.setMaxVelocity(1.2);
     command.setMaxAcceleration(1.0);
+    command.setOmegaLimit(1.0);
     return Status::RUNNING;
   });
 
@@ -358,6 +372,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
 
     command.setTargetTheta(pull_back_angle);
     command.setOmegaLimit(0.0);
+    command.setMaxVelocity(1.0);
     command.disablePlacementAvoidance();
     command.disableBallAvoidance();
     command.disableGoalAreaAvoidance();
@@ -367,8 +382,12 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
   });
 
   addTransition(
-    SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT,
-    [this]() { return skill_status == Status::SUCCESS; });
+    SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, [this]() {
+      Point placement_target;
+      placement_target << getParameter<double>("placement_x"), getParameter<double>("placement_y");
+      // ルール 5.2 0.15m以内で認められる。再配置が必要場合のみ、 ENTRY_POINTへ移動
+      return (world_model()->ball.pos - placement_target).norm() > 0.15;
+    });
 }
 
 void SingleBallPlacement::print(std::ostream & os) const
@@ -381,7 +400,7 @@ void SingleBallPlacement::print(std::ostream & os) const
       go_over_ball->print(os);
       break;
     case CONTACT_BALL:
-      get_ball_contact->print(os);
+      os << " CONTACT_BALL";
       break;
     case MOVE_TO_TARGET:
       move_with_ball->print(os);

diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp
@@ -70,7 +70,6 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U
   setUpSkillDictionary<skills::CmdLookAt>();
   setUpSkillDictionary<skills::CmdLookAtBall>();
   setUpSkillDictionary<skills::CmdLookAtBallFrom>();
-  setUpSkillDictionary<skills::GetBallContact>();
   //  setUpSkillDictionary<skills::Idle>();
   setUpSkillDictionary<skills::Goalie>();
   setUpSkillDictionary<skills::GoalKick>();

diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp
@@ -169,7 +169,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar
     .strokeWidth(20);
   visualizer_tracked->add(line_builder.getSvgString());
 
-  auto now = rclcpp::Clock().now();
+  auto now = rclcpp::Clock(RCL_ROS_TIME).now();
   const double corner_angle = std::acos(0.055 / 0.085);
   for (const auto & robot : world_model->ours.getAvailableRobots()) {
     SvgRobotBuilder builder;
@@ -192,7 +192,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar
     // ボールセンサ
     if (
       now.get_clock_type() == robot->ball_sensor_stamp.get_clock_type() &&
-      (now - robot->ball_sensor_stamp).seconds() < 1.0 && robot->ball_sensor) {
+      std::abs((now - robot->ball_sensor_stamp).seconds()) < 0.01 && robot->ball_sensor) {
       SvgLineBuilder ball_sensor_line;
       ball_sensor_line
         .start(robot->kicker_center() + getVerticalVec(getNormVec(robot->pose.theta)) * 0.055)

diff --git a/crane_world_model_publisher/src/world_model_data_provider.cpp b/crane_world_model_publisher/src/world_model_data_provider.cpp
@@ -61,7 +61,8 @@ WorldModelDataProvider::WorldModelDataProvider(rclcpp::Node & node)
           if (feedback->ball_sensor) {
             contact.last_contacted_time = now;
           }
-          data.ball_sensor_detected[robot.id] = feedback->ball_sensor;
+          // 範囲内参照で実行時エラー
+          // data.ball_sensor_detected[robot.id] = feedback->ball_sensor;
           auto & robot_info = data.robot_info[static_cast<uint8_t>(game_data.our_color)][robot.id];
           robot_info.ball_sensor = feedback->ball_sensor;
           robot_info.last_ball_sensor_stamp = now;

diff --git a/session/crane_planner_plugins/src/simple_ai_planner.cpp b/session/crane_planner_plugins/src/simple_ai_planner.cpp
@@ -43,7 +43,6 @@ SimpleAIPlanner::SimpleAIPlanner(WorldModelWrapper::SharedPtr & world_model, rcl
     setUpSkillDictionary<skills::CmdLookAt>();
     setUpSkillDictionary<skills::CmdLookAtBall>();
     setUpSkillDictionary<skills::CmdLookAtBallFrom>();
-    setUpSkillDictionary<skills::GetBallContact>();
     setUpSkillDictionary<skills::Idle>();
     setUpSkillDictionary<skills::Goalie>();
     setUpSkillDictionary<skills::GoalKick>();