From 13306f45fdec879081f070b7fe2b6618d8394ba6 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 15 Jan 2025 21:11:39 +0900 Subject: [PATCH 1/2] =?UTF-8?q?backward=5Fros=E3=82=92=E5=89=8A=E9=99=A4?= =?UTF-8?q?=20(#700)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/CMakeLists.txt | 4 ---- crane_local_planner/package.xml | 1 - 2 files changed, 5 deletions(-) diff --git a/crane_local_planner/CMakeLists.txt b/crane_local_planner/CMakeLists.txt index e70a0dc8c..cf6509d18 100755 --- a/crane_local_planner/CMakeLists.txt +++ b/crane_local_planner/CMakeLists.txt @@ -15,7 +15,6 @@ add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS") # find dependencies find_package(ament_cmake_auto REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(backward_ros REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME}_component SHARED @@ -40,9 +39,6 @@ ament_auto_add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp ) -include(/opt/ros/$ENV{ROS_DISTRO}/share/backward_ros/cmake/BackwardConfig.cmake) -add_backward(${PROJECT_NAME}_node) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/crane_local_planner/package.xml b/crane_local_planner/package.xml index f53e46002..17901fa12 100755 --- a/crane_local_planner/package.xml +++ b/crane_local_planner/package.xml @@ -9,7 +9,6 @@ ament_cmake_auto - backward_ros closest_point_vendor crane_msg_wrappers crane_msgs From f58c8ae2ab65e26f69a41c29263ab76bbe2c5e20 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 15 Jan 2025 22:04:42 +0900 Subject: [PATCH 2/2] =?UTF-8?q?attacker/=E3=82=AD=E3=83=83=E3=82=AF?= =?UTF-8?q?=E3=81=AE=E8=AA=BF=E6=95=B4=20(#701)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/attacker.hpp | 1 - .../include/crane_robot_skills/kick.hpp | 3 +- crane_robot_skills/src/attacker.cpp | 40 ++--- crane_robot_skills/src/kick.cpp | 166 ++++++------------ .../src/world_model_publisher.cpp | 1 - 5 files changed, 68 insertions(+), 143 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/attacker.hpp b/crane_robot_skills/include/crane_robot_skills/attacker.hpp index 8175dde83..d4e296e2a 100644 --- a/crane_robot_skills/include/crane_robot_skills/attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/attacker.hpp @@ -23,7 +23,6 @@ namespace crane::skills enum class AttackerState { ENTRY_POINT, FORCED_PASS, - CUT_THEIR_PASS, STEAL_BALL, REDIRECT_GOAL_KICK, GOAL_KICK, diff --git a/crane_robot_skills/include/crane_robot_skills/kick.hpp b/crane_robot_skills/include/crane_robot_skills/kick.hpp index f85c1daf0..9af627734 100644 --- a/crane_robot_skills/include/crane_robot_skills/kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kick.hpp @@ -17,8 +17,7 @@ namespace crane::skills enum class KickState { ENTRY_POINT, CHASE_BALL, - AROUND_BALL, - KICK, + AROUND_BALL_AND_KICK, REDIRECT_KICK, POSITIVE_REDIRECT_KICK, }; diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index e1ffe9700..8ed7430b4 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -60,7 +60,6 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) // パス command.disableBallAvoidance(); kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("kick_power", 0.8); int receiver_id = getParameter("receiver_id"); if (receiver_id != -1) { kick_target = world_model()->getOurRobot(receiver_id)->pose.pos; @@ -68,14 +67,25 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) kick_skill.setParameter("target", kick_target); Segment kick_line{world_model()->ball.pos, kick_target}; // 近くに敵ロボットがいればチップキック + bool chip_kick = false; if (const auto enemy_robots = world_model()->theirs.getAvailableRobots(); not enemy_robots.empty()) { const auto & [nearest_enemy, enemy_distance] = world_model()->getNearestRobotWithDistanceFromSegment(kick_line, enemy_robots); if (enemy_distance < 0.4 && nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) { - kick_skill.setParameter("kick_with_chip", true); + chip_kick = true; } } + if (chip_kick) { + kick_skill.setParameter("kick_with_chip", true); + kick_skill.setParameter("kick_power", 0.9); + kick_skill.setParameter("with_dribble", true); + kick_skill.setParameter("dribble_power", 0.7); + } else { + kick_skill.setParameter("kick_power", 0.2); + kick_skill.setParameter("kick_with_chip", false); + kick_skill.setParameter("dribble_power", 0.0); + } kick_skill.run(); break; } @@ -85,23 +95,6 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return Status::RUNNING; }); - addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool { - return not world_model()->isOurBallByBallOwnerCalculator() && - world_model()->ball.isMoving(0.2) && - world_model()->ball.isMovingTowards(robot()->pose.pos); - }); - - addTransition(AttackerState::CUT_THEIR_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { - return world_model()->isOurBallByBallOwnerCalculator() or world_model()->ball.isStopped(0.2); - }); - - addStateFunction(AttackerState::CUT_THEIR_PASS, [this]() -> Status { - visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5); - receive_skill.setParameter("enable_redirect", false); - receive_skill.setParameter("policy", std::string("min_slack")); - return receive_skill.run(); - }); - addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool { // 止まっているボールを相手が持っているとき const auto enemy_robots = world_model()->theirs.getAvailableRobots(); @@ -168,10 +161,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) } }(); - receive_skill.setParameter("enable_redirect", false); + receive_skill.setParameter("enable_redirect", true); receive_skill.setParameter("redirect_target", target); receive_skill.setParameter("policy", std::string("closest")); - receive_skill.setParameter("redirect_kick_power", 0.8); + receive_skill.setParameter("redirect_kick_power", 0.2); return receive_skill.run(); }); @@ -288,7 +281,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) kick_skill.setParameter("kick_with_chip", true); } } - kick_skill.setParameter("kick_power", 0.5); + kick_skill.setParameter("kick_power", 0.4); kick_skill.setParameter("dot_threshold", 0.97); return kick_skill.run(); }); @@ -361,7 +354,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) }); addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status { - receive_skill.setParameter("enable_redirect", true); + receive_skill.setParameter("enable_redirect", false); + receive_skill.setParameter("policy", std::string("closest")); receive_skill.setParameter("dribble_power", 0.0); receive_skill.setParameter("enable_software_bumper", false); return receive_skill.run(); diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index 59075d9dd..f3d92f1b6 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -15,7 +15,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) phase(getContextReference("phase")) { setParameter("target", Point(0, 0)); - setParameter("kick_power", 0.5f); + setParameter("kick_power", 0.7f); setParameter("chip_kick", false); setParameter("with_dribble", false); setParameter("dribble_power", 0.3f); @@ -39,51 +39,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) return Status::RUNNING; }); - addTransition(KickState::ENTRY_POINT, KickState::CHASE_BALL, [this]() { - return world_model()->ball.isMoving(getParameter("moving_speed_threshold")); - }); - - addTransition(KickState::ENTRY_POINT, KickState::AROUND_BALL, [this]() { return true; }); - - addStateFunction(KickState::CHASE_BALL, [this]() { - std::stringstream state; - state << "Kick::CHASE_BALL::"; - // メモ：ボールが近い時はボールから少しずらした位置を目指したほうがいいかも - auto [min_slack_pos, max_slack_pos] = - world_model()->getMinMaxSlackInterceptPoint({robot()}, 5.0, 0.1, -0.3, 1., 2.0); - if (min_slack_pos) { - state << "min_slack: " << min_slack_pos.value().x() << ", " << min_slack_pos.value().y(); - command.setTargetPosition(min_slack_pos.value()).lookAtBallFrom(min_slack_pos.value()); - } else { - // ball_lineとフィールドラインの交点を目指す - Point ball_exit_point = getBallExitPointFromField(0.3); - command.setTargetPosition(ball_exit_point) - .lookAtFrom(world_model()->ball.pos, ball_exit_point); - state << "ball_exit: " << ball_exit_point.x() << ", " << ball_exit_point.y(); - } - visualizer->addPoint(robot()->pose.pos, 0, "", 1., state.str()); - return Status::RUNNING; - }); - - addTransition(KickState::CHASE_BALL, KickState::AROUND_BALL, [this]() { - // ボールが止まったら回り込みへ - command.disableBallAvoidance(); - return not world_model()->ball.isMoving(getParameter("moving_speed_threshold")); - }); - - addTransition(KickState::CHASE_BALL, KickState::REDIRECT_KICK, [this]() { - // ボールライン上に乗ったらリダイレクトキックへ - command.disableBallAvoidance(); - return world_model()->ball.isMovingTowards(robot()->pose.pos, 10.0) && - getAngleDiff( - getAngle(world_model()->ball.vel), - getAngle(getParameter("target") - robot()->pose.pos) < M_PI / 2.0); - }); - - addTransition(KickState::CHASE_BALL, KickState::POSITIVE_REDIRECT_KICK, [this]() { - command.disableBallAvoidance(); - return world_model()->ball.isMovingTowards(robot()->pose.pos, 10.0); - }); + addTransition(KickState::ENTRY_POINT, KickState::AROUND_BALL_AND_KICK, [this]() { return true; }); addStateFunction(KickState::POSITIVE_REDIRECT_KICK, [this]() { visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK"); @@ -132,7 +88,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) return receive_skill->update(); }); - addTransition(KickState::REDIRECT_KICK, KickState::AROUND_BALL, [this]() { + addTransition(KickState::REDIRECT_KICK, KickState::AROUND_BALL_AND_KICK, [this]() { // ボールが止まったら回り込みへ return not world_model()->ball.isMoving(getParameter("moving_speed_threshold")); }); @@ -143,86 +99,64 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 30.); }); - addStateFunction(KickState::AROUND_BALL, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL"); + addStateFunction(KickState::AROUND_BALL_AND_KICK, [this]() { auto target = getParameter("target"); Point ball_pos = world_model()->ball.pos; - - // 経由ポイント - Point intermediate_point = - ball_pos + (ball_pos - target).normalized() * getParameter("around_interval"); - command.setTargetPosition(intermediate_point) - .setTerminalVelocity(0.1) - .lookAtFrom(target, ball_pos) - .enableCollisionAvoidance(); - - // ボールを避けて回り込む - if (((robot()->pose.pos - ball_pos).normalized()).dot((target - ball_pos).normalized()) > 0.1) { - Point around_point = [&]() { - Vector2 vertical_vec = getVerticalVec((target - ball_pos).normalized()) * - getParameter("around_interval"); - Point around_point1 = ball_pos + vertical_vec; - Point around_point2 = ball_pos - vertical_vec; - if (robot()->getDistance(around_point1) < robot()->getDistance(around_point2)) { - return around_point1; - } else { - return around_point2; - } - }(); - command.setTargetPosition(around_point).lookAtFrom(target, ball_pos); - } else { - // 経由ポイントへGO - command.setTargetPosition(intermediate_point) + visualizer->addLine(ball_pos, ball_pos + (target - ball_pos).normalized() * 1.0, 1, "blue"); + constexpr double SWITCH_DISTANCE = 1.0; + visualizer->addCircle(ball_pos, SWITCH_DISTANCE, 1, "yellow", "yellow", 0.); + if (robot()->getDistance(ball_pos) > SWITCH_DISTANCE) { + visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL(遠い)"); + command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3) .lookAtFrom(target, ball_pos) - .enableCollisionAvoidance(); - } - return Status::RUNNING; - }); - - addTransition(KickState::AROUND_BALL, KickState::KICK, [this]() { - // 中間地点に到達したらキックへ - Point intermediate_point = - world_model()->ball.pos + - (world_model()->ball.pos - getParameter("target")).normalized() * - getParameter("around_interval"); - return robot()->getDistance(intermediate_point) < 0.05 && robot()->vel.linear.norm() < 0.15; - }); - - addStateFunction(KickState::KICK, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::KICK"); - auto target = getParameter("target"); - Point ball_pos = world_model()->ball.pos; - command.setTargetPosition(ball_pos + (target - ball_pos).normalized() * 0.1) - .setTerminalVelocity(0.5) - .disableCollisionAvoidance() - .disableBallAvoidance(); - if (getParameter("chip_kick")) { - command.kickWithChip(getParameter("kick_power")); - } else { - command.kickStraight(getParameter("kick_power")); - } - if (getParameter("with_dribble")) { - command.dribble(getParameter("dribble_power")); + .setTerminalVelocity(0.3); + return Status::RUNNING; } else { - // ドリブラーを止める - command.withDribble(0.0); + visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL（近い）"); + auto calculateRatio = + [](const double distance, const double min_distance, const double max_distance) { + return (distance - min_distance) / (max_distance - min_distance); + }; + + // ボールを避けて回り込む + using boost::math::constants::degree; + double ratio = + 1.5 + std::clamp( + -calculateRatio(robot()->getDistance(world_model()->ball.pos), 0.2, 1.5), -0.5, 0.); + + double move_direction = getAngle(target - robot()->pose.pos) + + (getAngleDiff( + getAngle(world_model()->ball.pos - robot()->pose.pos), + getAngle(target - robot()->pose.pos))) * + ratio; + Vector2 move_vec = getNormVec(move_direction); + command.lookAtFrom(target, ball_pos) + .setDribblerTargetPosition( + robot()->pose.pos + move_vec * 0.3 + world_model()->ball.vel * 0.4) + // .setTerminalVelocity(world_model()->ball.vel.norm()) + .disableCollisionAvoidance() + .disableBallAvoidance(); + + if (getParameter("chip_kick")) { + command.kickWithChip(getParameter("kick_power")); + } else { + command.kickStraight(getParameter("kick_power")); + } + if (getParameter("with_dribble")) { + command.withDribble(getParameter("dribble_power")); + } else { + // ドリブラーを止める + command.withDribble(0.0); + } + return Status::RUNNING; } - return Status::RUNNING; }); - addTransition(KickState::KICK, KickState::ENTRY_POINT, [this]() { + addTransition(KickState::AROUND_BALL_AND_KICK, KickState::ENTRY_POINT, [this]() { // 素早く遠ざかっていったら終了 return world_model()->ball.isMoving(getParameter("kicked_speed_threshold")) && world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 30.); }); - - addTransition(KickState::KICK, KickState::ENTRY_POINT, [this]() -> bool { - // 素早く遠ざかっていったら終了 - auto target = getParameter("target"); - Point ball_pos = world_model()->ball.pos; - Point p = ball_pos + (target - ball_pos).normalized() * 0.3; - return robot()->getDistance(p) < 0.1; - }); } auto Kick::getBallExitPointFromField(const double offset) -> Point { diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 92061b146..44ffc5c96 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -275,7 +275,6 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame & int ball_info_history_size = ball_info_history.size(); if (1 > ball_info_history_size) { auto last_ball_info_history = ball_info_history[ball_info_history_size - 1]; - // double elapsed_time_since_last_detected = tracked_frame.timestamp() - last_ball_info_history.detection_time; double elapsed_time_since_last_detected = (now() - last_ball_detect_time).seconds(); // 0.5secビジョンから見えていなければ見失った