From 64603d1052945d00206cbd1a14050195d0ac0d0a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 13:06:39 +0900 Subject: [PATCH 01/39] =?UTF-8?q?rvo=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8A?= =?UTF-8?q?=E5=86=85=E3=81=ABPID=E3=82=92=E3=82=BB=E3=83=83=E3=83=88?= =?UTF-8?q?=E3=82=A2=E3=83=83=E3=83=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_local_planner/rvo2_planner.hpp | 10 +++++ crane_local_planner/src/rvo2_planner.cpp | 43 ++++++++++++++++++- 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp b/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp index 2490d0d0a..adaa87780 100644 --- a/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -59,6 +60,15 @@ class RVO2Planner : public LocalPlannerBase double ACCELERATION = 4.0; // 減速度は加速度の何倍にするかという係数 ParameterWithEvent deceleration_factor; + + std::array vx_controllers; + std::array vy_controllers; + + ParameterWithEvent p_gain; + ParameterWithEvent i_gain; + ParameterWithEvent d_gain; + + double I_SATURATION = 0.0; }; } // namespace crane #endif // CRANE_LOCAL_PLANNER__RVO2_PLANNER_HPP_ diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index a60496626..6fb7144db 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -14,7 +14,10 @@ namespace crane { RVO2Planner::RVO2Planner(rclcpp::Node & node) : LocalPlannerBase("rvo2_local_planner", node), - deceleration_factor("deceleration_factor", node, 1.5) + deceleration_factor("deceleration_factor", node, 1.5), + p_gain("p_gain", node, 4.0), + i_gain("i_gain", node, 0.0), + d_gain("d_gain", node, 0.0) { node.declare_parameter("rvo_time_step", RVO_TIME_STEP); RVO_TIME_STEP = node.get_parameter("rvo_time_step").as_double(); @@ -43,6 +46,44 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node) node.declare_parameter("max_acc", ACCELERATION); ACCELERATION = node.get_parameter("max_acc").as_double(); + p_gain.callback = [&](double value) { + for (auto & controller : vx_controllers) { + controller.setGain(value, i_gain.getValue(), d_gain.getValue()); + } + for (auto & controller : vy_controllers) { + controller.setGain(value, i_gain.getValue(), d_gain.getValue()); + } + }; + + i_gain.callback = [&](double value) { + for (auto & controller : vx_controllers) { + controller.setGain(p_gain.getValue(), value, d_gain.getValue()); + } + for (auto & controller : vy_controllers) { + controller.setGain(p_gain.getValue(), value, d_gain.getValue()); + } + }; + + d_gain.callback = [&](double value) { + for (auto & controller : vx_controllers) { + controller.setGain(p_gain.getValue(), i_gain.getValue(), value); + } + for (auto & controller : vy_controllers) { + controller.setGain(p_gain.getValue(), i_gain.getValue(), value); + } + }; + + node.declare_parameter("i_saturation", I_SATURATION); + I_SATURATION = node.get_parameter("i_saturation").as_double(); + + for (auto & controller : vx_controllers) { + controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION); + } + + for (auto & controller : vy_controllers) { + controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION); + } + rvo_sim = std::make_unique( RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, RVO_RADIUS, RVO_MAX_SPEED); From 4c7ed295ed1cfbc5de29af9e87fb968fa480beaf Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 13:13:05 +0900 Subject: [PATCH 02/39] =?UTF-8?q?rvo=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8A?= =?UTF-8?q?=E5=86=85=E3=81=ABPID=E3=81=AB=E3=82=88=E3=82=8B=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E5=88=B6=E9=99=90=E3=82=92=E6=B8=9B=E9=80=9F=E6=99=82?= =?UTF-8?q?=E3=81=AE=E3=81=BF=E5=B0=8E=E5=85=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/src/rvo2_planner.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index 6fb7144db..b3204b1be 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -143,7 +143,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 * deceleration * target_vel.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); + }(); // v = v0 + at double max_vel_by_acc = pre_vel + acceleration * RVO_TIME_STEP; From 28195ff91a686ea773e9af1319217b2f2a023def Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 15:37:43 +0900 Subject: [PATCH 03/39] =?UTF-8?q?STOP=E6=99=82=E3=81=AE30cm=E3=83=AB?= =?UTF-8?q?=E3=83=BC=E3=83=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/src/rvo2_planner.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index b3204b1be..a0a7e41e4 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -288,8 +288,12 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg) }(); if (not command.local_planner_config.disable_goal_area_avoidance) { bool is_in_penalty_area = isInBox(penalty_area, target_pos, 0.2); - constexpr double SURROUNDING_OFFSET = 0.3; - constexpr double PENALTY_AREA_OFFSET = 0.1; + double SURROUNDING_OFFSET = 0.3; + double PENALTY_AREA_OFFSET = 0.1; + if(world_model->play_situation.getSituationCommandID()== crane_msgs::msg::PlaySituation::STOP) { + PENALTY_AREA_OFFSET = 0.4; + SURROUNDING_OFFSET = 0.6; + } if (isInBox( penalty_area, Point(command.current_pose.x, command.current_pose.y), PENALTY_AREA_OFFSET)) { From e21a8dfafe957e764e50e88226c969623eb7a629 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 15:38:00 +0900 Subject: [PATCH 04/39] =?UTF-8?q?=E3=82=AD=E3=83=BC=E3=83=91=E3=83=BC?= =?UTF-8?q?=E3=82=92=E5=B0=91=E3=81=97=E5=89=8D=E3=81=AB=E5=87=BA=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/goalie.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 43ce0e545..70a455fd7 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -130,7 +130,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & const double BLOCK_DIST = getParameter("block_distance"); phase += "ボールを待ち受ける"; // デフォルト位置設定 - command.setTargetPosition(world_model()->getOurGoalCenter()).lookAt(Point(0, 0)); + command.setTargetPosition(world_model()->getOurGoalCenter() * 0.9).lookAt(Point(0, 0)); if (std::signbit(world_model()->ball.pos.x()) == std::signbit(world_model()->goal.x())) { phase += " (自コート警戒モード)"; Segment ball_prediction_4s(ball.pos, ball.pos + ball.vel * 4.0); From a14dbcd20224e3025eb570d52fc1c3a3762c5b97 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 15:59:43 +0900 Subject: [PATCH 05/39] =?UTF-8?q?STOP=E3=81=A7=E5=8F=96=E3=82=8B=E8=B7=9D?= =?UTF-8?q?=E9=9B=A2=E3=82=92=E5=A4=A7=E3=81=8D=E3=81=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/ball_nearby_positioner.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/include/crane_robot_skills/ball_nearby_positioner.hpp b/crane_robot_skills/include/crane_robot_skills/ball_nearby_positioner.hpp index 09dd563fb..dec8f044b 100644 --- a/crane_robot_skills/include/crane_robot_skills/ball_nearby_positioner.hpp +++ b/crane_robot_skills/include/crane_robot_skills/ball_nearby_positioner.hpp @@ -28,7 +28,7 @@ class BallNearByPositioner : public SkillBase setParameter("positioning_policy", std::string("goal")); // 整列距離 setParameter("robot_interval", 0.3); - setParameter("margin_distance", 0.3); + setParameter("margin_distance", 0.6); } Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override From 4bd0367d4aca5c27558cdbc788f80bcbd88ee01a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 16:00:39 +0900 Subject: [PATCH 06/39] =?UTF-8?q?=E3=82=AD=E3=83=BC=E3=83=91=E3=83=BC?= =?UTF-8?q?=E3=81=AE=E7=88=86=E5=8A=A0=E9=80=9F=E3=82=92OFF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/goalie.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 70a455fd7..85a17865c 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -220,7 +220,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & command.setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command.getRobot()->getDistance(wait_point) > 0.03) { // なりふり構わず爆加速 - command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); +// command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } phase += "(パスカットモードFRONT)"; } else if (penalty_area_pass_to_side) { @@ -229,7 +229,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & command.setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command.getRobot()->getDistance(wait_point) > 0.03) { // なりふり構わず爆加速 - command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); +// command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } phase += "(パスカットモードSIDE)"; } @@ -262,7 +262,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & command.setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command.getRobot()->getDistance(wait_point) > 0.03) { // なりふり構わず爆加速 - command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); +// command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } } } From ef37f6386800c3d8d0d7baebc5c3698465661b0b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 16:08:31 +0900 Subject: [PATCH 07/39] =?UTF-8?q?=E6=99=82=E9=96=93=E3=81=AE=E5=8D=98?= =?UTF-8?q?=E4=BD=8D=E3=82=92=E7=A2=BA=E8=AA=8D=E3=80=82ns=E3=81=A0?= =?UTF-8?q?=E3=81=A3=E3=81=9F=E3=81=AE=E3=82=92ms=E3=81=AB=E3=81=97?= =?UTF-8?q?=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 2e4e7b8c4..e6eb697a4 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -150,9 +150,9 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba bool is_corner = std::abs(ball_pos.x()) > (field.x() - 0.05) && std::abs(ball_pos.y()) > (field.y() - 0.05); std::cout << "is_corner: " << is_corner - << ", contact duration: " << robot()->ball_contact.getContactDuration().count() + << ", contact duration: " << robot()->ball_contact.getContactDuration().count() / 1e6 << std::endl; - return is_corner && robot()->ball_contact.getContactDuration().count() > 0.5; + return is_corner && robot()->ball_contact.getContactDuration().count() / 1e6 > 500; }); // 失敗の場合は最初に戻る From 0dc46159f93a8198e14e17e7206a6e20e723b4b0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 16:11:50 +0900 Subject: [PATCH 08/39] =?UTF-8?q?SingleBallPlacementStates::PULL=5FBACK=5F?= =?UTF-8?q?FROM=5FEDGE=5FPULL=E3=81=AE=E5=88=B6=E9=99=90=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E3=82=92=E4=B8=8B=E3=81=92=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index e6eb697a4..5bb58d4ec 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -174,7 +174,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.setDribblerTargetPosition(pull_back_target.value()); // 角度はそのまま引っ張りたいので指定はしない command.dribble(0.6); - command.setMaxVelocity(0.3); + command.setMaxVelocity(0.15); command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableBallAvoidance(); From 8b94a77a7bc97a202b9bc9b7d2cc72ed8dcd2bbb Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 17:00:59 +0900 Subject: [PATCH 09/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=AE=E5=9B=9E=E9=81=BF=E3=81=8C?= =?UTF-8?q?=E8=B6=B3=E3=82=8A=E3=81=AA=E3=81=8B=E3=81=A3=E3=81=9F=E3=81=AE?= =?UTF-8?q?=E3=81=A720cm=E8=BF=BD=E5=8A=A0=E3=81=A7=E5=9B=9E=E9=81=BF?= =?UTF-8?q?=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/placement_avoidance_planner.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp index 4e1e39400..4d36a75a7 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp @@ -62,11 +62,11 @@ class BallPlacementAvoidancePlanner : public PlannerBase world_model->getBallPlacementArea().value().segment, command.original_position); // 0.6m離れる Point target_position = - closest_point + (command.original_position - closest_point).normalized() * 0.6; + closest_point + (command.original_position - closest_point).normalized() * 0.8; if (not world_model->point_checker.isFieldInside(target_position, 0.2)) { // 一番近いフィールド外のポイントがだめなので逆方向に0.6m離れる target_position = - closest_point + (closest_point - command.original_position).normalized() * 0.6; + closest_point + (closest_point - command.original_position).normalized() * 0.8; if (auto segment = world_model->getBallPlacementArea().value().segment; (closest_point == segment.first || closest_point == segment.second)) { @@ -74,7 +74,7 @@ class BallPlacementAvoidancePlanner : public PlannerBase // 垂直方向に0.6m離れた点を複数選択して、フィールド外かつ配置エリア外の点を選択する std::vector target_candidates; Vector2 vertical_vec = - getVerticalVec((segment.second - segment.first).normalized()) * 0.6; + getVerticalVec((segment.second - segment.first).normalized()) * 0.8; target_candidates.push_back(closest_point + vertical_vec); target_candidates.push_back(closest_point - vertical_vec); From ae388ccdeaea68af13c9c00f4889f07406e4f3f7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 17:01:21 +0900 Subject: [PATCH 10/39] =?UTF-8?q?=E3=81=82=E3=82=93=E3=81=BE=E3=82=8A?= =?UTF-8?q?=E5=9B=9E=E3=82=89=E3=81=AA=E3=81=84=E3=81=AE=E3=81=A7=E3=81=A8?= =?UTF-8?q?=E3=82=8A=E3=81=82=E3=81=88=E3=81=9Aomega=5Flimit=E3=82=9210?= =?UTF-8?q?=E5=80=8D=E3=81=AB=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/msg/control/RobotCommand.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_msgs/msg/control/RobotCommand.msg b/crane_msgs/msg/control/RobotCommand.msg index 14e19d1a1..5abca865c 100755 --- a/crane_msgs/msg/control/RobotCommand.msg +++ b/crane_msgs/msg/control/RobotCommand.msg @@ -29,7 +29,7 @@ geometry_msgs/Pose2D current_pose bool enable_local_feedback float32 target_theta -float32 omega_limit 10.0 +float32 omega_limit 100.0 # for local planner float32 theta_tolerance 0.0 From ebd3e15b59914b3ccf6eb168ef334e286f319ca1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 17:01:54 +0900 Subject: [PATCH 11/39] =?UTF-8?q?=E3=81=BE=E3=81=A0=E3=83=9A=E3=83=8A?= =?UTF-8?q?=E3=83=AB=E3=83=86=E3=82=A3=E3=82=A8=E3=83=AA=E3=82=A2=E3=81=AB?= =?UTF-8?q?=E8=BF=91=E3=81=A5=E3=81=8D=E3=81=99=E3=81=8E=E3=82=8B=E3=81=AE?= =?UTF-8?q?=E3=81=A7=E6=9B=B4=E3=81=AB=E3=83=9E=E3=83=BC=E3=82=B8=E3=83=B3?= =?UTF-8?q?=E3=82=92=E3=81=A8=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/src/rvo2_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index a0a7e41e4..5d611330f 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -291,7 +291,7 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg) double SURROUNDING_OFFSET = 0.3; double PENALTY_AREA_OFFSET = 0.1; if(world_model->play_situation.getSituationCommandID()== crane_msgs::msg::PlaySituation::STOP) { - PENALTY_AREA_OFFSET = 0.4; + PENALTY_AREA_OFFSET = 0.5; SURROUNDING_OFFSET = 0.6; } if (isInBox( From 9e427023bbb4e52674fbd45e7990f635fdfe2a88 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 17:02:13 +0900 Subject: [PATCH 12/39] =?UTF-8?q?=E3=82=AD=E3=83=83=E3=82=AF=E3=82=AA?= =?UTF-8?q?=E3=83=95=E3=81=8C=E5=BC=B7=E3=81=99=E3=81=8E=E3=82=8B=E3=81=AE?= =?UTF-8?q?=E3=81=A7=E3=80=81=E5=B0=91=E3=81=97=E5=BC=B1=E3=81=8F=E3=81=99?= =?UTF-8?q?=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/kickoff_attack.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/kickoff_attack.cpp b/crane_robot_skills/src/kickoff_attack.cpp index b98b7211a..a39d48e87 100644 --- a/crane_robot_skills/src/kickoff_attack.cpp +++ b/crane_robot_skills/src/kickoff_attack.cpp @@ -13,7 +13,7 @@ KickoffAttack::KickoffAttack(RobotCommandWrapperBase::SharedPtr & base) { setParameter("target_x", 0.0f); setParameter("target_y", 1.0f); - setParameter("kick_power", 0.3); + setParameter("kick_power", 0.25); addStateFunction( KickoffAttackState::PREPARE_KICKOFF, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { From 91c1930cdd866a75bacf4c25a5285cda164ef68f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 18:28:45 +0900 Subject: [PATCH 13/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E6=88=90=E5=8A=9F=E6=9D=A1=E4=BB=B6?= =?UTF-8?q?=E3=81=8C=E3=82=AC=E3=83=90=E3=82=AC=E3=83=90=E3=81=A0=E3=81=A3?= =?UTF-8?q?=E3=81=9F=E3=81=AE=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 5bb58d4ec..604171049 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -32,7 +32,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba [this]() { auto placement_target = world_model()->getBallPlacementTarget(); if ( - placement_target && bg::distance(world_model()->ball.pos, placement_target.value()) > 0.5) { + placement_target && bg::distance(world_model()->ball.pos, placement_target.value()) > 0.1) { return true; } else { // 動かす必要がなければそのまま @@ -42,7 +42,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba addTransition( SingleBallPlacementStates::ENTRY_POINT, SingleBallPlacementStates::LEAVE_BALL, [this]() { - if (bg::distance(world_model()->ball.pos, robot()->pose.pos) < 0.5) { + if (bg::distance(world_model()->ball.pos, robot()->pose.pos) < 0.1) { // ボールに近すぎたら離れる return true; } else { From 07d8bfbe415ea4f6fedc0bda505d34684d1a1b09 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 18:29:07 +0900 Subject: [PATCH 14/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A7=E3=81=91=E3=82=8B=E3=81=AE?= =?UTF-8?q?=E3=81=8C=E5=BC=B7=E3=81=99=E3=81=8E=E3=81=9F=E3=81=AE=E3=82=92?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 604171049..db13a4108 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -128,7 +128,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.dribble(0.5); } else { // 角ではない場合は蹴る - command.kickStraight(0.5); + command.kickStraight(0.3); } return skill_status; }); From 0c8e0565dc564f180e1d53c5394300b6768b7f53 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 18:29:34 +0900 Subject: [PATCH 15/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A7=E7=A7=BB=E5=8B=95=E7=B5=82?= =?UTF-8?q?=E3=82=8F=E3=81=A3=E3=81=9F=E5=BE=8C=E3=81=AB=E3=82=88=E3=82=8A?= =?UTF-8?q?=E9=95=B7=E3=81=8F=E5=BE=85=E3=81=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index db13a4108..3b8986bbf 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -302,7 +302,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); if (not sleep) { sleep = std::make_shared(command_base); - sleep->setParameter("duration", 1.0); + sleep->setParameter("duration", 2.0); } skill_status = sleep->run(visualizer); command.stopHere(); From e64abf4eacda6c7989585568d821cc355181691f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 12 Oct 2024 18:29:59 +0900 Subject: [PATCH 16/39] =?UTF-8?q?=E8=A7=92=E9=80=9F=E5=BA=A6=E5=88=B6?= =?UTF-8?q?=E9=99=90=E3=81=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/msg/control/LocalPlannerConfig.msg | 2 +- crane_sender/src/ibis_sender_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_msgs/msg/control/LocalPlannerConfig.msg b/crane_msgs/msg/control/LocalPlannerConfig.msg index 283c35dbf..c175fe2ad 100644 --- a/crane_msgs/msg/control/LocalPlannerConfig.msg +++ b/crane_msgs/msg/control/LocalPlannerConfig.msg @@ -6,6 +6,6 @@ bool disable_rule_area_avoidance false float32 max_acceleration 4.0 float32 max_velocity 6.0 -float32 max_omega 1.0 +float32 max_omega 10.0 float32 terminal_velocity 0 uint8 priority 0 diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index dbab3393a..b00fa492d 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -152,7 +152,7 @@ class IbisSenderNode : public SenderBase packet.stop_emergency = command.stop_flag; packet.acceleration_limit = command.local_planner_config.max_acceleration; packet.linear_velocity_limit = command.local_planner_config.max_velocity; - packet.angular_velocity_limit = command.local_planner_config.max_omega; + packet.angular_velocity_limit = 10.; packet.prioritize_move = true; packet.prioritize_accurate_acceleration = true; From b785c36f8769dfe1fa0a900222dcd6d324c5f19f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 12 Oct 2024 14:27:40 +0000 Subject: [PATCH 17/39] style(pre-commit): autofix --- crane_local_planner/src/rvo2_planner.cpp | 8 +++++--- crane_robot_skills/src/goalie.cpp | 6 +++--- crane_robot_skills/src/single_ball_placement.cpp | 5 ++--- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index 5d611330f..d18ca68a7 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -146,8 +146,8 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms // double max_vel_by_decel = std::sqrt(2.0 * deceleration * target_vel.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.); + 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); }(); @@ -290,7 +290,9 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg) bool is_in_penalty_area = isInBox(penalty_area, target_pos, 0.2); double SURROUNDING_OFFSET = 0.3; double PENALTY_AREA_OFFSET = 0.1; - if(world_model->play_situation.getSituationCommandID()== crane_msgs::msg::PlaySituation::STOP) { + if ( + world_model->play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::STOP) { PENALTY_AREA_OFFSET = 0.5; SURROUNDING_OFFSET = 0.6; } diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 85a17865c..a1dc65ca6 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -220,7 +220,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & command.setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command.getRobot()->getDistance(wait_point) > 0.03) { // なりふり構わず爆加速 -// command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); + // command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } phase += "(パスカットモードFRONT)"; } else if (penalty_area_pass_to_side) { @@ -229,7 +229,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & command.setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command.getRobot()->getDistance(wait_point) > 0.03) { // なりふり構わず爆加速 -// command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); + // command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } phase += "(パスカットモードSIDE)"; } @@ -262,7 +262,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & command.setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command.getRobot()->getDistance(wait_point) > 0.03) { // なりふり構わず爆加速 -// command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); + // command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } } } diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 3b8986bbf..0f2b6ae71 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -149,9 +149,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // ボールが角にある場合は引っ張る bool is_corner = std::abs(ball_pos.x()) > (field.x() - 0.05) && std::abs(ball_pos.y()) > (field.y() - 0.05); - std::cout << "is_corner: " << is_corner - << ", contact duration: " << robot()->ball_contact.getContactDuration().count() / 1e6 - << std::endl; + std::cout << "is_corner: " << is_corner << ", contact duration: " + << robot()->ball_contact.getContactDuration().count() / 1e6 << std::endl; return is_corner && robot()->ball_contact.getContactDuration().count() / 1e6 > 500; }); From d1ab8ab6a14f5ec435944d10999ddcb0a6b278d1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Oct 2024 08:01:27 +0900 Subject: [PATCH 18/39] Update ball_nearby_positioner.cpp --- crane_robot_skills/src/ball_nearby_positioner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/ball_nearby_positioner.cpp b/crane_robot_skills/src/ball_nearby_positioner.cpp index 9311c83f2..912d07811 100644 --- a/crane_robot_skills/src/ball_nearby_positioner.cpp +++ b/crane_robot_skills/src/ball_nearby_positioner.cpp @@ -20,7 +20,7 @@ BallNearByPositioner::BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & setParameter("positioning_policy", std::string("goal")); // 整列距離 setParameter("robot_interval", 0.3); - setParameter("margin_distance", 0.3); + setParameter("margin_distance", 0.6); } Status BallNearByPositioner::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) From fa93d00833994f9bb878d1cbb63685cbbd2400b7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Oct 2024 11:08:55 +0900 Subject: [PATCH 19/39] =?UTF-8?q?=E7=88=86=E5=8A=A0=E9=80=9F=E6=AE=8B?= =?UTF-8?q?=E3=81=A3=E3=81=A6=E3=81=9F=E3=81=82=E3=81=82=E3=81=82=E3=81=82?= =?UTF-8?q?=E3=81=82=E3=81=82=E3=81=82=E3=81=82=E3=81=82=E3=81=82=E3=81=82?= =?UTF-8?q?=E3=81=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/goalie.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index a1dc65ca6..a848e6e2a 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -116,7 +116,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & command.setTargetPosition(target).lookAtBallFrom(target); if (command.getRobot()->getDistance(target) > 0.05) { // なりふり構わず爆加速 - command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); + // command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } } else { if ( From 850aebec84bdf90f143eb92e7f5226b9b63ccb0f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Oct 2024 11:09:39 +0900 Subject: [PATCH 20/39] omega_limit --- crane_msgs/msg/control/RobotCommand.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_msgs/msg/control/RobotCommand.msg b/crane_msgs/msg/control/RobotCommand.msg index 5abca865c..14e19d1a1 100755 --- a/crane_msgs/msg/control/RobotCommand.msg +++ b/crane_msgs/msg/control/RobotCommand.msg @@ -29,7 +29,7 @@ geometry_msgs/Pose2D current_pose bool enable_local_feedback float32 target_theta -float32 omega_limit 100.0 +float32 omega_limit 10.0 # for local planner float32 theta_tolerance 0.0 From ecd7b282889c1ff921a5aaa44c217668eea17e00 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Oct 2024 15:21:11 +0900 Subject: [PATCH 21/39] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E3=83=9C?= =?UTF-8?q?=E3=83=BC=E3=83=AB=E5=9B=9E=E9=81=BF=E3=82=92OFF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 1 + crane_robot_skills/src/kick.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 96877875b..9d1f8e600 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -339,6 +339,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) kick_skill.setParameter("kick_power", 0.8); kick_skill.setParameter("dot_threshold", 0.95); kick_skill.setParameter("kick_with_chip", true); + command.disableBallAvoidance(); return kick_skill.run(visualizer); }); diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index cb9ef12a8..862cfab8b 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -134,6 +134,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) } else { receive_skill->setParameter("policy", std::string("min_slack")); } + command.disableBallAvoidance(); return receive_skill->update(visualizer); }); From 1f05e3579e63c06a1a15cbad86c3bf4109b00116 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Oct 2024 15:21:50 +0900 Subject: [PATCH 22/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=AE=E8=BE=BA=E3=82=AD=E3=83=83?= =?UTF-8?q?=E3=82=AF=E3=82=92=E5=BC=B1=E3=81=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 0f2b6ae71..8d7dfc6e7 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -128,7 +128,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.dribble(0.5); } else { // 角ではない場合は蹴る - command.kickStraight(0.3); + command.kickStraight(0.1); } return skill_status; }); From c3c67c34ecd33b299ef935fa7eb894f0ff761e9f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Oct 2024 15:22:43 +0900 Subject: [PATCH 23/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=AE=E5=AE=89=E5=AE=9A=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 8d7dfc6e7..830c36bd3 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -272,15 +272,15 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.5); - move_with_ball->setParameter("ball_stabilizing_time", 1.5); + move_with_ball->setParameter("dribble_power", 0.3); + move_with_ball->setParameter("ball_stabilizing_time", 3.); } skill_status = move_with_ball->run(visualizer); command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableRuleAreaAvoidance(); - command.setMaxVelocity(1.0); + command.setMaxVelocity(0.5); command.setMaxAcceleration(1.0); return Status::RUNNING; }); From 7436ed891b6a0ae4219f9b9000a96d7cb397ca6a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Oct 2024 15:24:09 +0900 Subject: [PATCH 24/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=AE=E8=A7=92=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E8=A8=AD=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 830c36bd3..0d1882882 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -24,6 +24,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); command.stopHere(); + command.setOmegaLimit(10.0); return Status::RUNNING; }); @@ -309,6 +310,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disableGoalAreaAvoidance(); command.disableBallAvoidance(); command.disableRuleAreaAvoidance(); + command.setOmegaLimit(0.0); return Status::RUNNING; }); @@ -334,6 +336,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba set_target_position->setParameter("reach_threshold", 0.05); command.setTargetTheta(pull_back_angle); + command.setOmegaLimit(0.0); command.disablePlacementAvoidance(); command.disableBallAvoidance(); command.disableGoalAreaAvoidance(); From 2e9f35573cde65e6ce6e2ab47ab95154443231cc Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:24:50 +0900 Subject: [PATCH 25/39] =?UTF-8?q?=E3=83=AD=E3=83=9C=E3=83=83=E3=83=88?= =?UTF-8?q?=E3=81=8C=E9=80=86=E3=82=92=E5=90=91=E3=81=8F=E5=95=8F=E9=A1=8C?= =?UTF-8?q?=E3=81=AE=E5=AF=BE=E5=87=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/kick.cpp | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index 862cfab8b..056de5781 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -51,16 +51,16 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) std::stringstream state; state << "Kick::CHASE_BALL::"; // メモ：ボールが近い時はボールから少しずらした位置を目指したほうがいいかも - auto [min_slack_pos, max_slack_pos] = world_model()->getMinMaxSlackInterceptPoint( - {robot()}, 5.0, 0.1, -0.1, command.getMsg().local_planner_config.max_acceleration, - command.getMsg().local_planner_config.max_velocity); + auto [min_slack_pos, max_slack_pos] = + world_model()->getMinMaxSlackInterceptPoint({robot()}, 5.0, 0.1, -0.1, 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).lookAtBallFrom(ball_exit_point); + command.setTargetPosition(ball_exit_point) + .lookAtFrom(world_model()->ball.pos, ball_exit_point); state << "ball_exit: " << ball_exit_point.x() << ", " << ball_exit_point.y(); } command.enableBallAvoidance(); @@ -159,9 +159,8 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) Point intermediate_point = ball_pos + (ball_pos - target).normalized() * getParameter("around_interval"); command.setTargetPosition(intermediate_point) - .lookAtBallFrom(intermediate_point) - .enableCollisionAvoidance() - .enableBallAvoidance(); + .lookAtFrom(target, ball_pos) + .enableCollisionAvoidance(); // ボールを避けて回り込む if ( @@ -177,13 +176,12 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) return around_point2; } }(); - command.setTargetPosition(around_point).lookAtBallFrom(around_point); + command.setTargetPosition(around_point).lookAtFrom(target, ball_pos); } else { // 経由ポイントへGO command.setTargetPosition(intermediate_point) - .lookAtBallFrom(intermediate_point) - .enableCollisionAvoidance() - .enableBallAvoidance(); + .lookAtFrom(target, ball_pos) + .enableCollisionAvoidance(); } return Status::RUNNING; }); From e1c3aa23dbaf4b7af4b758316dda3d10e559fdbd Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:25:40 +0900 Subject: [PATCH 26/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A72=E5=9B=9E=E7=9B=AE?= =?UTF-8?q?=E4=BB=A5=E9=99=8D=E3=81=AE=E3=83=AA=E3=83=88=E3=83=A9=E3=82=A4?= =?UTF-8?q?=E3=81=A7=E3=82=B9=E3=83=AA=E3=83=BC=E3=83=97=E3=81=97=E3=81=AA?= =?UTF-8?q?=E3=81=84=E5=95=8F=E9=A1=8C=E3=81=AE=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/include/crane_robot_skills/sleep.hpp | 2 ++ crane_robot_skills/src/single_ball_placement.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/sleep.hpp b/crane_robot_skills/include/crane_robot_skills/sleep.hpp index de5b4edee..bb428e922 100644 --- a/crane_robot_skills/include/crane_robot_skills/sleep.hpp +++ b/crane_robot_skills/include/crane_robot_skills/sleep.hpp @@ -24,6 +24,8 @@ class Sleep : public SkillBase double getRestTime() const; + void reset() { is_started = false; } + bool & is_started; std::chrono::time_point start_time; diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 0d1882882..65813286b 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -287,8 +287,12 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addTransition( - SingleBallPlacementStates::MOVE_TO_TARGET, SingleBallPlacementStates::SLEEP, - [this]() { return skill_status == Status::SUCCESS; }); + SingleBallPlacementStates::MOVE_TO_TARGET, SingleBallPlacementStates::SLEEP, [this]() { + if (sleep) { + sleep.reset(); + } + return skill_status == Status::SUCCESS; + }); // ボールが離れたら始めに戻る addTransition( From 17c9ea56df8c000644c889c1408a1f31523ac404 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:27:05 +0900 Subject: [PATCH 27/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A7=E5=88=9D=E6=9C=9F=E4=BD=8D?= =?UTF-8?q?=E7=BD=AE=E4=BF=AE=E6=AD=A3=E3=81=8C=E8=AA=A4=E7=88=86=E3=81=99?= =?UTF-8?q?=E3=82=8B=E3=81=AE=E3=81=A7=E4=B8=80=E5=BA=A6=E3=82=B3=E3=83=A1?= =?UTF-8?q?=E3=83=B3=E3=83=88=E3=82=A2=E3=82=A6=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/single_ball_placement.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 65813286b..4b62838e0 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -41,15 +41,15 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba } }); - addTransition( - SingleBallPlacementStates::ENTRY_POINT, SingleBallPlacementStates::LEAVE_BALL, [this]() { - if (bg::distance(world_model()->ball.pos, robot()->pose.pos) < 0.1) { - // ボールに近すぎたら離れる - return true; - } else { - return false; - } - }); + // addTransition( + // SingleBallPlacementStates::ENTRY_POINT, SingleBallPlacementStates::LEAVE_BALL, [this]() { + // if (bg::distance(world_model()->ball.pos, robot()->pose.pos) < 0.1) { + // // ボールに近すぎたら離れる + // return true; + // } else { + // return false; + // } + // }); // 端にある場合、コート側からアプローチする addStateFunction( From d84acbbea115a2003a03d5a3d76f4ac0aa9d79a4 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:27:36 +0900 Subject: [PATCH 28/39] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A7=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 4b62838e0..7872844ad 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -117,8 +117,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba .disableGoalAreaAvoidance() .disableRuleAreaAvoidance(); command.setTargetPosition(world_model()->ball.pos); - command.setTerminalVelocity(0.5); - command.setMaxVelocity(1.0); + command.setMaxVelocity(0.5); const auto & ball_pos = world_model()->ball.pos; const Vector2 field = world_model()->field_size * 0.5; @@ -129,7 +128,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.dribble(0.5); } else { // 角ではない場合は蹴る - command.kickStraight(0.1); + command.dribble(0.2); + command.kickStraight(0.15); } return skill_status; }); @@ -275,6 +275,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball->setParameter("target_y", getParameter("placement_y")); move_with_ball->setParameter("dribble_power", 0.3); move_with_ball->setParameter("ball_stabilizing_time", 3.); + move_with_ball->setParameter("reach_threshold", 0.2); } skill_status = move_with_ball->run(visualizer); From 5a1abe1700b217e270c2634bb8284bddbde3e9c4 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:28:07 +0900 Subject: [PATCH 29/39] =?UTF-8?q?=E3=83=86=E3=82=B9=E3=83=88=E7=94=A8?= =?UTF-8?q?=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8A=E3=83=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/planners.hpp | 1 + .../crane_planner_plugins/test_planner.hpp | 49 +++++++++++++++++++ .../src/test_planner.cpp | 43 ++++++++++++++++ .../config/play_situation/TEST.yaml | 7 +++ 4 files changed, 100 insertions(+) create mode 100644 session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp create mode 100644 session/crane_planner_plugins/src/test_planner.cpp create mode 100644 session/crane_session_controller/config/play_situation/TEST.yaml diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp index 4fb6065f5..6d05b829c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp @@ -22,6 +22,7 @@ #include "placement_avoidance_planner.hpp" #include "simple_placer_planner.hpp" #include "skill_planner.hpp" +#include "test_planner.hpp" // #include "temporary/ball_placement_planner.hpp" #include "their_penalty_kick_planner.hpp" #include "tigers_goalie_planner.hpp" diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp new file mode 100644 index 000000000..3f07dcb88 --- /dev/null +++ b/session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp @@ -0,0 +1,49 @@ +// Copyright (c) 2024 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_PLANNER_PLUGINS__TEST_PLANNER_HPP_ +#define CRANE_PLANNER_PLUGINS__TEST_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "visibility_control.h" + +namespace crane +{ +class TestPlanner : public PlannerBase +{ +public: + COMPOSITION_PUBLIC explicit TestPlanner( + WorldModelWrapper::SharedPtr & world_model, + const ConsaiVisualizerWrapper::SharedPtr & visualizer) + : PlannerBase("Test", world_model, visualizer) + { + } + + std::pair> calculateRobotCommand( + const std::vector & robots) override; + + auto getSelectedRobots( + [[maybe_unused]] uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector override; +}; + +} // namespace crane +#endif // CRANE_PLANNER_PLUGINS__TEST_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/src/test_planner.cpp b/session/crane_planner_plugins/src/test_planner.cpp new file mode 100644 index 000000000..55d44bfdd --- /dev/null +++ b/session/crane_planner_plugins/src/test_planner.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2024 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. + +#include + +namespace crane +{ +std::pair> +TestPlanner::calculateRobotCommand(const std::vector & robots) +{ + std::vector robot_commands; + static int count = 0; + count++; + for (auto robot_id : robots) { + auto command = std::make_shared( + "test_planner", robot_id.robot_id, world_model); + if (count % (60 * 10) > 60 * 5) { + command->setTargetPosition(Point(2.0, 2.0)); + } else { + command->setTargetPosition(Point(2.0, -2.0)); + } + robot_commands.emplace_back(command->getMsg()); + } + return {PlannerBase::Status::RUNNING, robot_commands}; +} + +auto TestPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + // choose id smaller first + return 15. - static_cast(-robot->id); + }, + prev_roles); + return selected; +} +} // namespace crane diff --git a/session/crane_session_controller/config/play_situation/TEST.yaml b/session/crane_session_controller/config/play_situation/TEST.yaml new file mode 100644 index 000000000..c333ae98d --- /dev/null +++ b/session/crane_session_controller/config/play_situation/TEST.yaml @@ -0,0 +1,7 @@ +name: TEST +description: TEST +sessions: + - name: test + capacity: 1 + - name: waiter + capacity: 20 From 6061d5c832ba369436540bd047e1a0d9476e2d55 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:28:31 +0900 Subject: [PATCH 30/39] =?UTF-8?q?=E3=82=AD=E3=83=83=E3=82=AF=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E3=81=AE=E6=9C=80=E4=BD=8E=E8=A6=81=E6=B1=82=E7=B2=BE?= =?UTF-8?q?=E5=BA=A6=E3=82=92=E4=B8=8B=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 9d1f8e600..85bacd151 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -196,6 +196,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) AttackerState::GOAL_KICK, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { goal_kick_skill.setParameter("dot_threshold", 0.95); + goal_kick_skill.setParameter("キック角度の最低要求精度[deg]", 5.0); return goal_kick_skill.run(visualizer); }); From e69de6fd16c03a37155534e866823fa81fda8086 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:28:53 +0900 Subject: [PATCH 31/39] =?UTF-8?q?kick=E3=82=B9=E3=82=AD=E3=83=AB=E3=81=AE?= =?UTF-8?q?=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/kick.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index 056de5781..22fb895a5 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -63,7 +63,6 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) .lookAtFrom(world_model()->ball.pos, ball_exit_point); state << "ball_exit: " << ball_exit_point.x() << ", " << ball_exit_point.y(); } - command.enableBallAvoidance(); visualizer->addPoint(robot()->pose.pos, 0, "", 1., state.str()); return Status::RUNNING; }); @@ -192,7 +191,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) 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.1; + return robot()->getDistance(intermediate_point) < 0.10 && robot()->vel.linear.norm() < 0.15; }); addStateFunction(KickState::KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { From 5540263c14b9ad876cad59dc35d34e38c7963c7a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 09:29:01 +0900 Subject: [PATCH 32/39] =?UTF-8?q?=E3=83=86=E3=82=B9=E3=83=88=E7=94=A8?= =?UTF-8?q?=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8A=E3=83=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_planner_plugins/planners.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp index 6d05b829c..3a3887081 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp @@ -73,6 +73,8 @@ auto generatePlanner(const std::string & planner_name, Ts... ts) -> PlannerBase: return std::make_shared(ts...); } else if (planner_name == "simple_placer") { return std::make_shared(ts...); + } else if (planner_name == "test") { + return std::make_shared(ts...); } else { throw std::runtime_error("Unknown planner name: " + planner_name); } From 97122f611d4845432c525d22e755e7ba01515ffe Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 12:23:32 +0900 Subject: [PATCH 33/39] =?UTF-8?q?=E3=83=8F=E3=83=BC=E3=83=95=E3=82=B3?= =?UTF-8?q?=E3=83=BC=E3=83=88=E7=B7=B4=E7=BF=92=E7=94=A8=E3=83=9C=E3=83=BC?= =?UTF-8?q?=E3=83=AB=E3=83=95=E3=82=A3=E3=83=AB=E3=82=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai_ros2/consai_vision_tracker/src/ball_tracker.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/consai_ros2/consai_vision_tracker/src/ball_tracker.cpp b/consai_ros2/consai_vision_tracker/src/ball_tracker.cpp index c2bf71000..9ba8a528f 100644 --- a/consai_ros2/consai_vision_tracker/src/ball_tracker.cpp +++ b/consai_ros2/consai_vision_tracker/src/ball_tracker.cpp @@ -231,6 +231,10 @@ void BallTracker::reset_prior() bool BallTracker::is_outlier(const TrackedBall & observation) const { + // ハーフコート練習用 + // if(observation.pos.x < 0) { + // return true; + // } // 観測が外れ値かどうか判定する // Reference: https://myenigma.hatenablog.com/entry/20140825/1408975706 const double THRESHOLD = 5.99; // 自由度2、棄却率5%のしきい値 From c01a532c772732f448918e1ad6613806a968ca6f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 12:24:07 +0900 Subject: [PATCH 34/39] =?UTF-8?q?=E3=83=91=E3=82=B9=E5=85=88=E5=9B=BA?= =?UTF-8?q?=E5=AE=9A=E3=81=AE=E8=A7=A3=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 85bacd151..04386b7a4 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -20,7 +20,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) steal_ball_skill(base) { receive_skill.setParameter("policy", std::string("closest")); - setParameter("receiver_id", 0); + setParameter("receiver_id", -1); addStateFunction( AttackerState::ENTRY_POINT, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { @@ -37,6 +37,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) game_command == crane_msgs::msg::PlaySituation::OUR_KICKOFF_START) { auto best_receiver = selectPassReceiver(); forced_pass_receiver_id = best_receiver->id; + setParameter("receiver_id", best_receiver->id); auto receiver = world_model()->getOurRobot(forced_pass_receiver_id); kick_skill.setParameter("target", receiver->pose.pos); forced_pass_phase = 1; @@ -62,8 +63,13 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) } case 1: { // パス + 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; + } Segment kick_line{world_model()->ball.pos, kick_target}; // 近くに敵ロボットがいればチップキック if (const auto enemy_robots = world_model()->theirs.getAvailableRobots(); @@ -226,6 +232,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) // int receiver_id = getParameter("receiver_id"); double best_score = 0.0; Point best_target; + int best_id = -1; for (auto & our_robot : our_robots) { Segment ball_to_target{world_model()->ball.pos, our_robot->pose.pos}; auto target = our_robot->pose.pos; @@ -258,12 +265,14 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) if (score > best_score) { best_score = score; best_target = target; + best_id = our_robot->id; } } auto ret = best_score > 0.5; if (ret) { kick_target = best_target; + setParameter("receiver_id", best_id); } return ret; }); @@ -276,6 +285,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) addStateFunction( AttackerState::STANDARD_PASS, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + int receiver_id = getParameter("receiver_id"); + if (receiver_id != -1) { + kick_target = world_model()->getOurRobot(receiver_id)->pose.pos; + } + auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); const auto enemy_robots = world_model()->theirs.getAvailableRobots(); From bcbc5b16eae60d952e0414ffca36c5aaf82bdbc4 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 12:24:41 +0900 Subject: [PATCH 35/39] =?UTF-8?q?=E3=83=86=E3=82=B9=E3=83=88=E3=82=92?= =?UTF-8?q?=E5=9B=9B=E8=A7=92=E3=81=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/src/test_planner.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/session/crane_planner_plugins/src/test_planner.cpp b/session/crane_planner_plugins/src/test_planner.cpp index 55d44bfdd..1ad07414e 100644 --- a/session/crane_planner_plugins/src/test_planner.cpp +++ b/session/crane_planner_plugins/src/test_planner.cpp @@ -17,10 +17,20 @@ TestPlanner::calculateRobotCommand(const std::vector & robots) for (auto robot_id : robots) { auto command = std::make_shared( "test_planner", robot_id.robot_id, world_model); - if (count % (60 * 10) > 60 * 5) { - command->setTargetPosition(Point(2.0, 2.0)); - } else { - command->setTargetPosition(Point(2.0, -2.0)); + int mode = (count % (30 * 10 * 4)) / (30 * 10); + switch (mode) { + case 0: + command->setTargetPosition(Point(3.5, 2.0)); + break; + case 1: + command->setTargetPosition(Point(3.5, -2.0)); + break; + case 2: + command->setTargetPosition(Point(-3.5, -2.0)); + break; + case 3: + command->setTargetPosition(Point(-3.5, 2.0)); + break; } robot_commands.emplace_back(command->getMsg()); } From f64f0c85444e90e52606a8603a85130cff3e4dfb Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 12:25:08 +0900 Subject: [PATCH 36/39] =?UTF-8?q?=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/kick.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index 22fb895a5..2e675baee 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -52,7 +52,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) state << "Kick::CHASE_BALL::"; // メモ：ボールが近い時はボールから少しずらした位置を目指したほうがいいかも auto [min_slack_pos, max_slack_pos] = - world_model()->getMinMaxSlackInterceptPoint({robot()}, 5.0, 0.1, -0.1, 1., 2.0); + 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()); @@ -191,7 +191,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) world_model()->ball.pos + (world_model()->ball.pos - getParameter("target")).normalized() * getParameter("around_interval"); - return robot()->getDistance(intermediate_point) < 0.10 && robot()->vel.linear.norm() < 0.15; + return robot()->getDistance(intermediate_point) < 0.05 && robot()->vel.linear.norm() < 0.15; }); addStateFunction(KickState::KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { From d2ddbe783b4b1658435c665e90158f249a0b9667 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 12:25:18 +0900 Subject: [PATCH 37/39] =?UTF-8?q?=E3=83=87=E3=83=BC=E3=82=BF=E7=94=A8launc?= =?UTF-8?q?h?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/data.launch.py | 109 ++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 crane_bringup/launch/data.launch.py diff --git a/crane_bringup/launch/data.launch.py b/crane_bringup/launch/data.launch.py new file mode 100644 index 000000000..696ad737e --- /dev/null +++ b/crane_bringup/launch/data.launch.py @@ -0,0 +1,109 @@ +# Copyright (c) 2024 ibis-ssl +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.conditions import IfCondition, UnlessCondition + +default_exit_behavior = Shutdown() + + +def generate_launch_description(): + return LaunchDescription( + [ + # Launch Arguments + DeclareLaunchArgument( + "vision_addr", + default_value="224.5.23.2", + description="Set multicast address to connect SSL-Vision.", + ), + DeclareLaunchArgument( + "vision_port", + default_value="10006", + description="Set multicast port to connect SSL-Vision.", + ), + DeclareLaunchArgument( + "referee_addr", + default_value="224.5.23.1", + description="Set multicast address to connect Game Controller.", + ), + DeclareLaunchArgument("referee_port", default_value="10003"), + # DeclareLaunchArgument("referee_port", default_value="11111"), + DeclareLaunchArgument("team", default_value="Test Team", description="team name"), + DeclareLaunchArgument( + "gui", default_value="true", description="Set true if you want to use GUI." + ), + Node( + package="robocup_ssl_comm", + executable="vision_node", + parameters=[ + {"multicast_address": LaunchConfiguration("vision_addr")}, + {"multicast_port": LaunchConfiguration("vision_port")}, + ], + on_exit=default_exit_behavior, + ), + Node( + package="robocup_ssl_comm", + executable="game_controller_node", + parameters=[ + {"multicast_address": LaunchConfiguration("referee_addr")}, + {"multicast_port": LaunchConfiguration("referee_port")}, + ], + on_exit=default_exit_behavior, + ), + Node( + package="crane_robot_receiver", + executable="robot_receiver_node", + output="screen", + # on_exit=default_exit_behavior, + ), + Node( + package="robocup_ssl_comm", + executable="robot_status_node", + parameters=[{"blue_port": 10311}, {"yellow_port": 10312}], + on_exit=default_exit_behavior, + ), + Node( + package="consai_vision_tracker", + executable="vision_tracker_node", + on_exit=default_exit_behavior, + ), + Node( + package="crane_world_model_publisher", + executable="crane_world_model_publisher_node", + parameters=[ + {"initial_team_color": "YELLOW"}, + {"team_name": LaunchConfiguration("team")}, + ], + on_exit=default_exit_behavior, + ), + Node( + package="crane_play_switcher", + executable="play_switcher_node", + output="screen", + parameters=[ + {"team_name": LaunchConfiguration("team")}, + ], + on_exit=Shutdown(), + ), + Node( + condition=IfCondition(LaunchConfiguration("gui")), + package="consai_visualizer", + executable="consai_visualizer", + on_exit=default_exit_behavior, + ), + ] + ) From 4d248e557dfce10ebbd32034a12de29dd724359c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 12:26:06 +0900 Subject: [PATCH 38/39] =?UTF-8?q?=E3=83=87=E3=83=BC=E3=82=BF=E7=94=A8launc?= =?UTF-8?q?h?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/crane.launch.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/crane_bringup/launch/crane.launch.py b/crane_bringup/launch/crane.launch.py index 6c83d3718..1cfa9884f 100644 --- a/crane_bringup/launch/crane.launch.py +++ b/crane_bringup/launch/crane.launch.py @@ -89,10 +89,10 @@ def generate_launch_description(): output="screen", parameters=[ {"planner": "rvo2"}, - {"p_gain": 2.0}, + {"p_gain": 5.0}, {"i_gain": 0.00}, {"i_saturation": 0.00}, - {"d_gain": 3.0}, + {"d_gain": 1.0}, {"max_vel": LaunchConfiguration("max_vel")}, {"max_acc": 3.0}, {"deceleration_factor": 1.5}, @@ -118,11 +118,12 @@ def generate_launch_description(): output="screen", parameters=[ {"planner": "rvo2"}, - {"p_gain": 3.0}, + {"p_gain": 5.5}, {"i_gain": 0.0}, {"i_saturation": 0.0}, - {"d_gain": 1.5}, + {"d_gain": 4.0}, {"max_vel": LaunchConfiguration("max_vel")}, + {"max_acc": 4.0}, {"deceleration_factor": 1.5}, ], on_exit=default_exit_behavior, @@ -157,7 +158,7 @@ def generate_launch_description(): {"no_movement": False}, {"latency_ms": 0.0}, {"sim_mode": LaunchConfiguration("sim")}, - {"kick_power_limit_straight": 1.0}, + {"kick_power_limit_straight": 0.30}, {"kick_power_limit_chip": 1.0}, ], on_exit=default_exit_behavior, @@ -213,8 +214,10 @@ def generate_launch_description(): package="crane_play_switcher", executable="play_switcher_node", output="screen", - parameters=[{"team_name": LaunchConfiguration("team")}], - on_exit=default_exit_behavior, + parameters=[ + {"team_name": LaunchConfiguration("team")}, + ], + on_exit=Shutdown(), ), # Group with speak condition GroupAction( @@ -223,7 +226,6 @@ def generate_launch_description(): Node( package="crane_speaker", executable="crane_speaker_node", - on_exit=default_exit_behavior, ) ], ), @@ -232,8 +234,8 @@ def generate_launch_description(): executable="speak_ros_node", parameters=[ {"plugin_name": "voicevox_plugin::VoiceVoxPlugin"}, - {"voicevox_plugin/speaker": 14}, - {"voicevox_plugin/speedScale": 1.0}, + {"voicevox_plugin/speaker": 13}, + {"voicevox_plugin/speedScale": 0.8}, {"voicevox_plugin/volumeScale": 1.0}, ], on_exit=default_exit_behavior, From 87138b94b5440ca37384b939fc3493bc353cc5f6 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 14 Oct 2024 12:26:25 +0900 Subject: [PATCH 39/39] =?UTF-8?q?=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/receive.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/receive.cpp b/crane_robot_skills/src/receive.cpp index c14efd3e4..b8eafe317 100644 --- a/crane_robot_skills/src/receive.cpp +++ b/crane_robot_skills/src/receive.cpp @@ -74,7 +74,7 @@ Point Receive::getInterceptionPoint(const ConsaiVisualizerWrapper::SharedPtr & v std::string policy = getParameter("policy"); if (policy.ends_with("slack")) { auto [min_slack, max_slack] = world_model()->getMinMaxSlackInterceptPoint( - {robot()}, 3.0, 0.1, 0., 4, 4, world_model()->getBallDistanceHorizon()); + {robot()}, 3.0, 0.1, -0.2, 1, 2, world_model()->getBallDistanceHorizon()); if (policy == "max_slack" && max_slack) { return max_slack.value(); } else if (policy == "min_slack" && min_slack) {