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..f6b646748 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 @@ -16,97 +16,9 @@ namespace crane::skills class BallNearByPositioner : public SkillBase { public: - explicit BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & base) - : SkillBase("BallNearByPositioner", base) - { - // このロボットのインデックス - setParameter("current_robot_index", 0); - setParameter("total_robot_number", 1); - // 整列ポリシー(arc/straight) - setParameter("line_policy", std::string("arc")); - // ボールの位置決めポリシー(goal/pass) - setParameter("positioning_policy", std::string("goal")); - // 整列距離 - setParameter("robot_interval", 0.3); - setParameter("margin_distance", 0.3); - } + explicit BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & base); - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override - { - auto situation = world_model()->play_situation.getSituationCommandID(); - double distance_from_ball = [&]() { - switch (situation) { - case crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE: - return 0.5; - case crane_msgs::msg::PlaySituation::THEIR_INDIRECT_FREE: - return 0.5; - case crane_msgs::msg::PlaySituation::STOP: - return 0.5; - case crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT: - return 0.5; - default: - return 0.0; - } - }(); - - distance_from_ball += getParameter("margin_distance"); - double normalized_offset = - (getParameter("total_robot_number") - getParameter("current_robot_index") - 1) / 2.; - double offset = normalized_offset * getParameter("robot_interval"); - Point base_position = - world_model()->ball.pos + - [&](const std::string & policy) { - if (policy == "goal") { - return (world_model()->getOurGoalCenter() - world_model()->ball.pos).normalized(); - } else if (policy == "pass") { - // 2番目に近いロボット - auto theirs = world_model()->theirs.getAvailableRobots(); - if (theirs.size() > 2) { - auto nearest_robot = - world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); - theirs.erase( - std::remove_if( - theirs.begin(), theirs.end(), - [&](const auto & r) { return r->id == nearest_robot.first->id; }), - theirs.end()); - auto second_nearest_robot = - world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); - return (second_nearest_robot.first->pose.pos - world_model()->ball.pos).normalized(); - } else { - throw std::runtime_error( - "[BallNearByPositioner] 「positioning policy: " - "pass」の計算には少なくとも2台の敵ロボットが必要です"); - } - } else { - throw std::runtime_error( - "[BallNearByPositioner] " - "予期しないパラメータ「positioning_policy」が入力されています: " + - policy); - } - }(getParameter("positioning_policy")) * - distance_from_ball; - - Point target_position = [&](const std::string & policy) -> Point { - Vector2 ball_to_base = (base_position - world_model()->ball.pos); - if (policy == "arc") { - double base_angle = getAngle(ball_to_base); - // r x theta = interval - // theta = interval / r - double angle_interval = getParameter("robot_interval") / distance_from_ball; - return world_model()->ball.pos + - getNormVec(base_angle + normalized_offset * angle_interval) * distance_from_ball; - } else if (policy == "straight") { - return ball_to_base + getVerticalVec(ball_to_base.normalized()) * offset; - } else { - throw std::runtime_error( - "[BallNearByPositioner] 予期しないパラメータ「line_policy」が入力されています: " + - policy); - } - }(getParameter("line_policy")); - - command.setTargetPosition(target_position).lookAtBall(); - return Status::RUNNING; - } + Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; void print(std::ostream & os) const override { os << "[BallNearByPositioner]"; } }; diff --git a/crane_robot_skills/include/crane_robot_skills/cut_pass.hpp b/crane_robot_skills/include/crane_robot_skills/cut_pass.hpp deleted file mode 100644 index b272c6d4f..000000000 --- a/crane_robot_skills/include/crane_robot_skills/cut_pass.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright (c) 2023 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#ifndef CRANE_ROBOT_SKILLS__CUT_PASS_HPP_ -#define CRANE_ROBOT_SKILLS__CUT_PASS_HPP_ - -#include -#include -#include - -namespace crane::skills -{ -class CutPass : public SkillBase -{ -public: - explicit CutPass(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("CutPass", base) - { - setParameter("dribble_power", 0.3); - setParameter("enable_software_bumper", true); - setParameter("software_bumper_start_time", 0.5); - // min_slack, max_slack, closest - setParameter("policy", std::string("closest")); - setParameter("enable_active_receive", true); - } - - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override - { - world_model().get for (auto their_robot : world_model()->theirs.getAvailableRobots()) - { - if (world_model()->ball.isMovingTowards(their_robot->pose.pos, 10.0, 0.5)) { - return Status::FAILURE; - } - } - auto offset = [&]() -> Point { - Point offset(0, 0); - if (getParameter("enable_software_bumper")) { - // ボール到着まで残り秒になったら、ボール速度方向に少し加速して衝撃を和らげる - double ball_speed = world_model()->ball.vel.norm(); - if ( - robot()->getDistance(world_model()->ball.pos) < - ball_speed * getParameter("software_bumper_start_time")) { - // ボールから逃げ切らないようにするため、速度の0.5倍に制限 - command.setMaxVelocity(ball_speed * 0.5); - // ボール速度方向に速度の0.5倍だけオフセット(1m/sで近づいていたら0.5m) - offset += world_model()->ball.vel.normalized() * (world_model()->ball.vel.norm() * 0.5); - } - } - if (getParameter("enable_active_receive")) { - if (world_model()->ball.isMovingTowards(robot()->pose.pos, 2.0, 0.5)) { - offset += (world_model()->ball.pos - robot()->pose.pos); - double distance = (world_model()->ball.pos - robot()->pose.pos).norm(); - command.setMaxVelocity(distance); - } - } - return offset; - }(); - auto interception_point = getInterceptionPoint() + offset; - command.lookAtBallFrom(interception_point) - .setDribblerTargetPosition(interception_point) - .dribble(getParameter("dribble_power")) - .disableBallAvoidance(); - - return Status::RUNNING; - } - - void print(std::ostream & os) const override { os << "[CutPass]"; } - - Point getInterceptionPoint() const - { - std::string policy = getParameter("policy"); - if (policy.ends_with("slack")) { - auto [max_slack_point, max_slack] = world_model()->getMinMaxSlackInterceptPoint({robot()}); - if (policy == "max_slack" && max_slack_point) { - return max_slack_point.value(); - } else if (policy == "min_slack" && max_slack_point) { - return max_slack_point.value(); - } - return world_model()->ball.pos; - } else if (policy == "closest") { - Segment ball_line( - world_model()->ball.pos, - (world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0)); - auto result = getClosestPointAndDistance(robot()->pose.pos, ball_line); - return result.closest_point; - } else { - throw std::runtime_error("Invalid policy for Receive::getInterceptionPoint: " + policy); - } - } -}; -} // namespace crane::skills -#endif // CRANE_ROBOT_SKILLS__CUT_PASS_HPP_ diff --git a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp index 0d27863a3..6d657dc0c 100644 --- a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp @@ -17,29 +17,9 @@ namespace crane::skills class GoalKick : public SkillBase { public: - explicit GoalKick(RobotCommandWrapperBase::SharedPtr & base) - : SkillBase("GoalKick", base), kick_skill(base) - { - setParameter("キック角度の最低要求精度[deg]", 1.0); - setParameter("dot_threshold", 0.95); - kick_skill.setParameter("kick_power", 0.8); - kick_skill.setParameter("chip_kick", false); - kick_skill.setParameter("with_dribble", false); - kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); - } + explicit GoalKick(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override - { - double best_angle = getBestAngleToShootFromPoint( - getParameter("キック角度の最低要求精度[deg]") * M_PI / 180., world_model()->ball.pos, - world_model()); - - Point target = world_model()->ball.pos + getNormVec(best_angle) * 0.5; - visualizer->addLine(world_model()->ball.pos, target, 2, "red"); - kick_skill.setParameter("target", target); - kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); - return kick_skill.run(visualizer); - } + Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; void print(std::ostream & os) const override { os << "[GoalKick] "; } @@ -47,43 +27,7 @@ class GoalKick : public SkillBase static double getBestAngleToShootFromPoint( double minimum_angle_accuracy, const Point from_point, - const WorldModelWrapper::SharedPtr & world_model) - { - auto [best_angle, goal_angle_width] = - world_model->getLargestGoalAngleRangeFromPoint(from_point); - if (goal_angle_width < 0.) { - // ゴールが見えない場合はgoal_angle_widthが負になる - // その場合は相手ゴール中心を狙う - best_angle = getAngle(world_model->getTheirGoalCenter() - from_point); - } - // 隙間のなかで更に良い角度を計算する。 - // キック角度の最低要求精度をオフセットとしてできるだけ端っこを狙う - if (goal_angle_width < minimum_angle_accuracy * 2.0) { - double best_angle1, best_angle2; - best_angle1 = best_angle - goal_angle_width / 2.0 + minimum_angle_accuracy; - best_angle2 = best_angle + goal_angle_width / 2.0 - minimum_angle_accuracy; - Point their_goalie_pos = [&]() -> Point { - const auto & enemy_robots = world_model->theirs.getAvailableRobots(); - if (not enemy_robots.empty()) { - return world_model - ->getNearestRobotWithDistanceFromPoint(world_model->getTheirGoalCenter(), enemy_robots) - .first->pose.pos; - } else { - return world_model->getTheirGoalCenter(); - } - }(); - double their_goalie_angle = getAngle(their_goalie_pos - from_point); - // 敵ゴールキーパーから角度差が大きい方を選択 - if ( - std::abs(getAngleDiff(their_goalie_angle, best_angle1)) < - std::abs(getAngleDiff(their_goalie_angle, best_angle2))) { - best_angle = best_angle2; - } else { - best_angle = best_angle1; - } - } - return best_angle; - } + const WorldModelWrapper::SharedPtr & world_model); }; } // namespace crane::skills #endif // CRANE_ROBOT_SKILLS__GOAL_KICK_HPP_ diff --git a/crane_robot_skills/include/crane_robot_skills/kick.hpp b/crane_robot_skills/include/crane_robot_skills/kick.hpp index bab4da60d..f85c1daf0 100644 --- a/crane_robot_skills/include/crane_robot_skills/kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kick.hpp @@ -29,258 +29,14 @@ class Kick : public SkillBaseWithState std::shared_ptr receive_skill; public: - explicit Kick(RobotCommandWrapperBase::SharedPtr & base) - : SkillBaseWithState("Kick", base, KickState::ENTRY_POINT), - receive_skill(std::make_shared(base)), - phase(getContextReference("phase")) - { - setParameter("target", Point(0, 0)); - setParameter("kick_power", 0.5f); - setParameter("chip_kick", false); - setParameter("with_dribble", false); - setParameter("dribble_power", 0.3f); - setParameter("dot_threshold", 0.95f); - setParameter("angle_threshold", 0.1f); - setParameter("around_interval", 0.3f); - setParameter("go_around_ball", true); - setParameter("moving_speed_threshold", 0.2); - setParameter("kicked_speed_threshold", 1.5); - - receive_skill->setParameter("dribble_power", 0.3); - receive_skill->setParameter("enable_software_bumper", false); - receive_skill->setParameter("policy", std::string("min_slack")); - receive_skill->setParameter("enable_active_receive", true); - receive_skill->setParameter("enable_redirect", true); - receive_skill->setParameter("redirect_target", Point(0, 0)); - receive_skill->setParameter("redirect_kick_power", 0.3); - - addStateFunction( - KickState::ENTRY_POINT, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::ENTRY_POINT"); - 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](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - 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); - 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); - 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; - }); - - 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); - }); - - addStateFunction( - KickState::POSITIVE_REDIRECT_KICK, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK"); - // ボールラインに沿って追いかけつつ、角度はtargetへ向ける - const auto & ball_pos = world_model()->ball.pos; - command.lookAtFrom(getParameter("target"), ball_pos); - - const auto & ball_vel_normed = world_model()->ball.vel.normalized(); - Segment ball_line{ball_pos - ball_vel_normed * 10, ball_pos + ball_vel_normed * 10}; - auto [distance, closest_point] = getClosestPointAndDistance(ball_pos, ball_line); - if ((ball_pos - closest_point).dot(ball_vel_normed) > 0) { - // 通り過ぎていれば追いかけて蹴る - auto target_pos = [&]() -> Point { - if (distance < 0.1) { - return ball_pos + ball_vel_normed; - } else { - return closest_point + ball_vel_normed * distance; - } - }(); - command.setDribblerTargetPosition(target_pos); - command.kickStraight(0.3); - command.disableBallAvoidance(); - } else { - // まだだったら避ける - command.setTargetPosition( - closest_point + (robot()->pose.pos - closest_point).normalized() * 0.3); - } - - return Status::RUNNING; - }); - - addTransition(KickState::POSITIVE_REDIRECT_KICK, KickState::ENTRY_POINT, [this]() { - return !world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 10.0) or - !world_model()->ball.isMovingTowards(getParameter("target"), 30.0); - }); - - addStateFunction( - KickState::REDIRECT_KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK"); - receive_skill->setParameter("target", getParameter("target")); - if (robot()->getDistance(world_model()->ball.pos) < 0.5) { - receive_skill->setParameter("policy", std::string("closest")); - } else { - receive_skill->setParameter("policy", std::string("min_slack")); - } - return receive_skill->update(visualizer); - }); - - addTransition(KickState::REDIRECT_KICK, KickState::AROUND_BALL, [this]() { - // ボールが止まったら回り込みへ - return not world_model()->ball.isMoving(getParameter("moving_speed_threshold")); - }); - - addTransition(KickState::REDIRECT_KICK, KickState::ENTRY_POINT, [this]() { - // 素早く遠ざかっていったら終了 - return world_model()->ball.isMoving(getParameter("kicked_speed_threshold")) && - world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 30.); - }); - - addStateFunction( - KickState::AROUND_BALL, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL"); - 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) - .lookAtBallFrom(intermediate_point) - .enableCollisionAvoidance() - .enableBallAvoidance(); - - // ボールを避けて回り込む - 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).lookAtBallFrom(around_point); - } else { - // 経由ポイントへGO - command.setTargetPosition(intermediate_point) - .lookAtBallFrom(intermediate_point) - .enableCollisionAvoidance() - .enableBallAvoidance(); - } - 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.1; - }); - - addStateFunction( - KickState::KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - 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.3) - .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")); - } else { - // ドリブラーを止める - command.withDribble(0.0); - } - return Status::RUNNING; - }); - - addTransition(KickState::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; - }); - } + explicit Kick(RobotCommandWrapperBase::SharedPtr & base); /** * @brief ボールがフィールドから出る位置を取得 * @param offset 出る位置を内側にずらすオフセット * @return ボールが出る位置 */ - auto getBallExitPointFromField(const double offset = 0.3) -> Point - { - Segment ball_line{ - world_model()->ball.pos, - world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0}; - - const double X = world_model()->field_size.x() / 2.0 - offset; - const double Y = world_model()->field_size.y() / 2.0 - offset; - - std::vector segments; - segments.emplace_back(Point(X, Y), Point(X, -Y)); - segments.emplace_back(Point(-X, Y), Point(-X, -Y)); - segments.emplace_back(Point(X, Y), Point(-X, Y)); - segments.emplace_back(Point(X, -Y), Point(-X, -Y)); - - for (const auto & seg : segments) { - if (auto intersections = getIntersections(ball_line, seg); not intersections.empty()) { - return intersections.front(); - } - } - return world_model()->ball.pos; - } + auto getBallExitPointFromField(const double offset = 0.3) -> Point; void print(std::ostream & os) const override { os << "[Kick]"; } diff --git a/crane_robot_skills/include/crane_robot_skills/teleop.hpp b/crane_robot_skills/include/crane_robot_skills/teleop.hpp index 47135b1b0..2f656cf1c 100644 --- a/crane_robot_skills/include/crane_robot_skills/teleop.hpp +++ b/crane_robot_skills/include/crane_robot_skills/teleop.hpp @@ -18,161 +18,9 @@ namespace crane::skills class Teleop : public SkillBase, public rclcpp::Node { public: - explicit Teleop(RobotCommandWrapperBase::SharedPtr & base) - : SkillBase("Teleop", base), Node("teleop_skill") - { - setParameter("rotation_deg", 0.); - setParameter("use_local_coordinate", false); - std::cout << "Teleop skill created" << std::endl; - joystick_subscription = this->create_subscription( - "/joy", 10, [this](const sensor_msgs::msg::Joy & msg) { - std::cout << "joy message received" << std::endl; - last_joy_msg = msg; - }); - std::cout << "joy subscriber created" << std::endl; - } + explicit Teleop(RobotCommandWrapperBase::SharedPtr & base); - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override - { - rclcpp::spin_some(this->get_node_base_interface()); - if (last_joy_msg.buttons.empty()) { - std::cout << "no joy message" << std::endl; - return Status::RUNNING; - } - - const int BUTTON_POWER_ENABLE = 9; - - const int AXIS_VEL_SURGE = 1; - const int AXIS_VEL_SWAY = 0; - const int AXIS_VEL_ANGULAR_R = 2; - const int AXIS_VEL_ANGULAR_L = 5; - - const int BUTTON_KICK_TOGGLE = 4; - const int BUTTON_KICK_STRAIGHT = 13; - const int BUTTON_KICK_CHIP = 14; - const int BUTTON_ADJUST_KICK = 0; - - const int BUTTON_ADJUST = 10; - const int BUTTON_ADJUST_UP = 11; - const int BUTTON_ADJUST_DOWN = 12; - - const int BUTTON_DRIBBLE_TOGGLE = 6; - const int BUTTON_ADJUST_DRIBBLE = 1; - - const double MAX_VEL_SURGE = 1.0; - const double MAX_VEL_SWAY = 1.0; - const double MAX_VEL_ANGULAR = M_PI * 0.1; - - static bool is_kick_mode_straight = true; - static bool is_kick_enable = false; - static bool is_dribble_enable = false; - - if (last_joy_msg.buttons[BUTTON_KICK_CHIP]) { - std::cout << "chip mode" << std::endl; - is_kick_mode_straight = false; - } - if (last_joy_msg.buttons[BUTTON_KICK_STRAIGHT]) { - std::cout << "straight mode" << std::endl; - is_kick_mode_straight = true; - } - - auto update_mode = [&](bool & mode_variable, const int button, bool & is_pushed) { - // trigger button up - if (last_joy_msg.buttons[button]) { - if (!is_pushed) { - std::cout << "toggle mode!" << std::endl; - mode_variable = not mode_variable; - } - is_pushed = true; - } else { - is_pushed = false; - } - }; - - static bool is_pushed_kick = false; - static bool is_pushed_dribble = false; - - update_mode(is_kick_enable, BUTTON_KICK_TOGGLE, is_pushed_kick); - update_mode(is_dribble_enable, BUTTON_DRIBBLE_TOGGLE, is_pushed_dribble); - - auto adjust_value = [](double & value, const double step) { - value += step; - value = std::clamp(value, 0.0, 1.0); - }; - - if (last_joy_msg.buttons[BUTTON_ADJUST]) { - static bool is_pushed = false; - if (last_joy_msg.buttons[BUTTON_ADJUST_UP]) { - // trigger button up - if (!is_pushed) { - if (last_joy_msg.buttons[BUTTON_ADJUST_KICK]) { - adjust_value(kick_power, 0.1); - std::cout << "kick up: " << kick_power << std::endl; - } - - if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { - adjust_value(dribble_power, 0.1); - std::cout << "dribble up:" << dribble_power << std::endl; - } - } - is_pushed = true; - } else if (last_joy_msg.buttons[BUTTON_ADJUST_DOWN]) { - // trigger button up - if (!is_pushed) { - if (last_joy_msg.buttons[BUTTON_ADJUST_KICK]) { - adjust_value(kick_power, -0.1); - std::cout << "kick down: " << kick_power << std::endl; - } - if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { - adjust_value(dribble_power, -0.1); - std::cout << "dribble down: " << dribble_power << std::endl; - } - } - is_pushed = true; - } else { - is_pushed = false; - } - } - - Point target = [&]() -> Point { - using boost::math::constants::degree; - double rotation_angle = getParameter("rotation_deg") * degree(); - if (getParameter("use_local_coordinate")) { - rotation_angle += robot()->pose.theta; - } - Eigen::Rotation2Dd rotation(rotation_angle); - return robot()->pose.pos + - rotation.toRotationMatrix() * Point{ - last_joy_msg.axes[AXIS_VEL_SURGE] * MAX_VEL_SURGE, - last_joy_msg.axes[AXIS_VEL_SWAY] * MAX_VEL_SWAY}; - }(); - - command.setTargetPosition(target); - - double angular = - (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_R]) - (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_L]); - - theta += angular; - command.setTargetTheta(normalizeAngle(theta)); - - if (is_kick_enable) { - if (is_kick_mode_straight) { - command.kickStraight(kick_power); - } else { - command.kickWithChip(kick_power); - } - } else { - command.kickStraight(0.0); - } - - if (is_dribble_enable) { - command.dribble(dribble_power); - } else { - command.dribble(0.0); - } - - return Status::RUNNING; - } + Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; void print(std::ostream & os) const override { os << "[Teleop]"; } diff --git a/crane_robot_skills/src/ball_nearby_positioner.cpp b/crane_robot_skills/src/ball_nearby_positioner.cpp new file mode 100644 index 000000000..9311c83f2 --- /dev/null +++ b/crane_robot_skills/src/ball_nearby_positioner.cpp @@ -0,0 +1,101 @@ +// 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::skills +{ +BallNearByPositioner::BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & base) +: SkillBase("BallNearByPositioner", base) +{ + // このロボットのインデックス + setParameter("current_robot_index", 0); + setParameter("total_robot_number", 1); + // 整列ポリシー(arc/straight) + setParameter("line_policy", std::string("arc")); + // ボールの位置決めポリシー(goal/pass) + setParameter("positioning_policy", std::string("goal")); + // 整列距離 + setParameter("robot_interval", 0.3); + setParameter("margin_distance", 0.3); +} + +Status BallNearByPositioner::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +{ + auto situation = world_model()->play_situation.getSituationCommandID(); + double distance_from_ball = [&]() { + switch (situation) { + case crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE: + return 0.5; + case crane_msgs::msg::PlaySituation::THEIR_INDIRECT_FREE: + return 0.5; + case crane_msgs::msg::PlaySituation::STOP: + return 0.5; + case crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT: + return 0.5; + default: + return 0.0; + } + }(); + + distance_from_ball += getParameter("margin_distance"); + double normalized_offset = + (getParameter("total_robot_number") - getParameter("current_robot_index") - 1) / 2.; + double offset = normalized_offset * getParameter("robot_interval"); + Point base_position = + world_model()->ball.pos + + [&](const std::string & policy) { + if (policy == "goal") { + return (world_model()->getOurGoalCenter() - world_model()->ball.pos).normalized(); + } else if (policy == "pass") { + // 2番目に近いロボット + auto theirs = world_model()->theirs.getAvailableRobots(); + if (theirs.size() > 2) { + auto nearest_robot = + world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); + theirs.erase( + std::remove_if( + theirs.begin(), theirs.end(), + [&](const auto & r) { return r->id == nearest_robot.first->id; }), + theirs.end()); + auto second_nearest_robot = + world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); + return (second_nearest_robot.first->pose.pos - world_model()->ball.pos).normalized(); + } else { + throw std::runtime_error( + "[BallNearByPositioner] 「positioning policy: " + "pass」の計算には少なくとも2台の敵ロボットが必要です"); + } + } else { + throw std::runtime_error( + "[BallNearByPositioner] " + "予期しないパラメータ「positioning_policy」が入力されています: " + + policy); + } + }(getParameter("positioning_policy")) * + distance_from_ball; + + Point target_position = [&](const std::string & policy) -> Point { + Vector2 ball_to_base = (base_position - world_model()->ball.pos); + if (policy == "arc") { + double base_angle = getAngle(ball_to_base); + // r x theta = interval + // theta = interval / r + double angle_interval = getParameter("robot_interval") / distance_from_ball; + return world_model()->ball.pos + + getNormVec(base_angle + normalized_offset * angle_interval) * distance_from_ball; + } else if (policy == "straight") { + return ball_to_base + getVerticalVec(ball_to_base.normalized()) * offset; + } else { + throw std::runtime_error( + "[BallNearByPositioner] 予期しないパラメータ「line_policy」が入力されています: " + policy); + } + }(getParameter("line_policy")); + + command.setTargetPosition(target_position).lookAtBall(); + return Status::RUNNING; +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/goal_kick.cpp b/crane_robot_skills/src/goal_kick.cpp new file mode 100644 index 000000000..c03c0be9d --- /dev/null +++ b/crane_robot_skills/src/goal_kick.cpp @@ -0,0 +1,74 @@ +// 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::skills +{ + +GoalKick::GoalKick(RobotCommandWrapperBase::SharedPtr & base) +: SkillBase("GoalKick", base), kick_skill(base) +{ + setParameter("キック角度の最低要求精度[deg]", 1.0); + setParameter("dot_threshold", 0.95); + kick_skill.setParameter("kick_power", 0.8); + kick_skill.setParameter("chip_kick", false); + kick_skill.setParameter("with_dribble", false); + kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); +} + +Status GoalKick::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +{ + double best_angle = getBestAngleToShootFromPoint( + getParameter("キック角度の最低要求精度[deg]") * M_PI / 180., world_model()->ball.pos, + world_model()); + + Point target = world_model()->ball.pos + getNormVec(best_angle) * 0.5; + visualizer->addLine(world_model()->ball.pos, target, 2, "red"); + kick_skill.setParameter("target", target); + kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); + return kick_skill.run(visualizer); +} + +double GoalKick::getBestAngleToShootFromPoint( + double minimum_angle_accuracy, const Point from_point, + const WorldModelWrapper::SharedPtr & world_model) +{ + auto [best_angle, goal_angle_width] = world_model->getLargestGoalAngleRangeFromPoint(from_point); + if (goal_angle_width < 0.) { + // ゴールが見えない場合はgoal_angle_widthが負になる + // その場合は相手ゴール中心を狙う + best_angle = getAngle(world_model->getTheirGoalCenter() - from_point); + } + // 隙間のなかで更に良い角度を計算する。 + // キック角度の最低要求精度をオフセットとしてできるだけ端っこを狙う + if (goal_angle_width < minimum_angle_accuracy * 2.0) { + double best_angle1, best_angle2; + best_angle1 = best_angle - goal_angle_width / 2.0 + minimum_angle_accuracy; + best_angle2 = best_angle + goal_angle_width / 2.0 - minimum_angle_accuracy; + Point their_goalie_pos = [&]() -> Point { + const auto & enemy_robots = world_model->theirs.getAvailableRobots(); + if (not enemy_robots.empty()) { + return world_model + ->getNearestRobotWithDistanceFromPoint(world_model->getTheirGoalCenter(), enemy_robots) + .first->pose.pos; + } else { + return world_model->getTheirGoalCenter(); + } + }(); + double their_goalie_angle = getAngle(their_goalie_pos - from_point); + // 敵ゴールキーパーから角度差が大きい方を選択 + if ( + std::abs(getAngleDiff(their_goalie_angle, best_angle1)) < + std::abs(getAngleDiff(their_goalie_angle, best_angle2))) { + best_angle = best_angle2; + } else { + best_angle = best_angle1; + } + } + return best_angle; +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp new file mode 100644 index 000000000..cb9ef12a8 --- /dev/null +++ b/crane_robot_skills/src/kick.cpp @@ -0,0 +1,255 @@ +// 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::skills +{ + +Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) +: SkillBaseWithState("Kick", base, KickState::ENTRY_POINT), + receive_skill(std::make_shared(base)), + phase(getContextReference("phase")) +{ + setParameter("target", Point(0, 0)); + setParameter("kick_power", 0.5f); + setParameter("chip_kick", false); + setParameter("with_dribble", false); + setParameter("dribble_power", 0.3f); + setParameter("dot_threshold", 0.95f); + setParameter("angle_threshold", 0.1f); + setParameter("around_interval", 0.3f); + setParameter("go_around_ball", true); + setParameter("moving_speed_threshold", 0.2); + setParameter("kicked_speed_threshold", 1.5); + + receive_skill->setParameter("dribble_power", 0.3); + receive_skill->setParameter("enable_software_bumper", false); + receive_skill->setParameter("policy", std::string("min_slack")); + receive_skill->setParameter("enable_active_receive", true); + receive_skill->setParameter("enable_redirect", true); + receive_skill->setParameter("redirect_target", Point(0, 0)); + receive_skill->setParameter("redirect_kick_power", 0.3); + + addStateFunction( + KickState::ENTRY_POINT, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { + visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::ENTRY_POINT"); + 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](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { + 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); + 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); + 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; + }); + + 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); + }); + + addStateFunction( + KickState::POSITIVE_REDIRECT_KICK, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { + visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK"); + // ボールラインに沿って追いかけつつ、角度はtargetへ向ける + const auto & ball_pos = world_model()->ball.pos; + command.lookAtFrom(getParameter("target"), ball_pos); + + const auto & ball_vel_normed = world_model()->ball.vel.normalized(); + Segment ball_line{ball_pos - ball_vel_normed * 10, ball_pos + ball_vel_normed * 10}; + auto [distance, closest_point] = getClosestPointAndDistance(ball_pos, ball_line); + if ((ball_pos - closest_point).dot(ball_vel_normed) > 0) { + // 通り過ぎていれば追いかけて蹴る + auto target_pos = [&]() -> Point { + if (distance < 0.1) { + return ball_pos + ball_vel_normed; + } else { + return closest_point + ball_vel_normed * distance; + } + }(); + command.setDribblerTargetPosition(target_pos); + command.kickStraight(0.3); + command.disableBallAvoidance(); + } else { + // まだだったら避ける + command.setTargetPosition( + closest_point + (robot()->pose.pos - closest_point).normalized() * 0.3); + } + + return Status::RUNNING; + }); + + addTransition(KickState::POSITIVE_REDIRECT_KICK, KickState::ENTRY_POINT, [this]() { + return !world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 10.0) or + !world_model()->ball.isMovingTowards(getParameter("target"), 30.0); + }); + + addStateFunction( + KickState::REDIRECT_KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { + visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK"); + receive_skill->setParameter("target", getParameter("target")); + if (robot()->getDistance(world_model()->ball.pos) < 0.5) { + receive_skill->setParameter("policy", std::string("closest")); + } else { + receive_skill->setParameter("policy", std::string("min_slack")); + } + return receive_skill->update(visualizer); + }); + + addTransition(KickState::REDIRECT_KICK, KickState::AROUND_BALL, [this]() { + // ボールが止まったら回り込みへ + return not world_model()->ball.isMoving(getParameter("moving_speed_threshold")); + }); + + addTransition(KickState::REDIRECT_KICK, KickState::ENTRY_POINT, [this]() { + // 素早く遠ざかっていったら終了 + return world_model()->ball.isMoving(getParameter("kicked_speed_threshold")) && + world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 30.); + }); + + addStateFunction( + KickState::AROUND_BALL, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { + visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL"); + 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) + .lookAtBallFrom(intermediate_point) + .enableCollisionAvoidance() + .enableBallAvoidance(); + + // ボールを避けて回り込む + 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).lookAtBallFrom(around_point); + } else { + // 経由ポイントへGO + command.setTargetPosition(intermediate_point) + .lookAtBallFrom(intermediate_point) + .enableCollisionAvoidance() + .enableBallAvoidance(); + } + 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.1; + }); + + addStateFunction(KickState::KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { + 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.3) + .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")); + } else { + // ドリブラーを止める + command.withDribble(0.0); + } + return Status::RUNNING; + }); + + addTransition(KickState::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 +{ + Segment ball_line{ + world_model()->ball.pos, world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0}; + + const double X = world_model()->field_size.x() / 2.0 - offset; + const double Y = world_model()->field_size.y() / 2.0 - offset; + + std::vector segments; + segments.emplace_back(Point(X, Y), Point(X, -Y)); + segments.emplace_back(Point(-X, Y), Point(-X, -Y)); + segments.emplace_back(Point(X, Y), Point(-X, Y)); + segments.emplace_back(Point(X, -Y), Point(-X, -Y)); + + for (const auto & seg : segments) { + if (auto intersections = getIntersections(ball_line, seg); not intersections.empty()) { + return intersections.front(); + } + } + return world_model()->ball.pos; +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/teleop.cpp b/crane_robot_skills/src/teleop.cpp new file mode 100644 index 000000000..10ee616d7 --- /dev/null +++ b/crane_robot_skills/src/teleop.cpp @@ -0,0 +1,166 @@ +// 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::skills +{ +Teleop::Teleop(RobotCommandWrapperBase::SharedPtr & base) +: SkillBase("Teleop", base), Node("teleop_skill") +{ + setParameter("rotation_deg", 0.); + setParameter("use_local_coordinate", false); + std::cout << "Teleop skill created" << std::endl; + joystick_subscription = this->create_subscription( + "/joy", 10, [this](const sensor_msgs::msg::Joy & msg) { + std::cout << "joy message received" << std::endl; + last_joy_msg = msg; + }); + std::cout << "joy subscriber created" << std::endl; +} + +Status Teleop::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +{ + rclcpp::spin_some(this->get_node_base_interface()); + if (last_joy_msg.buttons.empty()) { + std::cout << "no joy message" << std::endl; + return Status::RUNNING; + } + + const int BUTTON_POWER_ENABLE = 9; + + const int AXIS_VEL_SURGE = 1; + const int AXIS_VEL_SWAY = 0; + const int AXIS_VEL_ANGULAR_R = 2; + const int AXIS_VEL_ANGULAR_L = 5; + + const int BUTTON_KICK_TOGGLE = 4; + const int BUTTON_KICK_STRAIGHT = 13; + const int BUTTON_KICK_CHIP = 14; + const int BUTTON_ADJUST_KICK = 0; + + const int BUTTON_ADJUST = 10; + const int BUTTON_ADJUST_UP = 11; + const int BUTTON_ADJUST_DOWN = 12; + + const int BUTTON_DRIBBLE_TOGGLE = 6; + const int BUTTON_ADJUST_DRIBBLE = 1; + + const double MAX_VEL_SURGE = 1.0; + const double MAX_VEL_SWAY = 1.0; + const double MAX_VEL_ANGULAR = M_PI * 0.1; + + static bool is_kick_mode_straight = true; + static bool is_kick_enable = false; + static bool is_dribble_enable = false; + + if (last_joy_msg.buttons[BUTTON_KICK_CHIP]) { + std::cout << "chip mode" << std::endl; + is_kick_mode_straight = false; + } + if (last_joy_msg.buttons[BUTTON_KICK_STRAIGHT]) { + std::cout << "straight mode" << std::endl; + is_kick_mode_straight = true; + } + + auto update_mode = [&](bool & mode_variable, const int button, bool & is_pushed) { + // trigger button up + if (last_joy_msg.buttons[button]) { + if (!is_pushed) { + std::cout << "toggle mode!" << std::endl; + mode_variable = not mode_variable; + } + is_pushed = true; + } else { + is_pushed = false; + } + }; + + static bool is_pushed_kick = false; + static bool is_pushed_dribble = false; + + update_mode(is_kick_enable, BUTTON_KICK_TOGGLE, is_pushed_kick); + update_mode(is_dribble_enable, BUTTON_DRIBBLE_TOGGLE, is_pushed_dribble); + + auto adjust_value = [](double & value, const double step) { + value += step; + value = std::clamp(value, 0.0, 1.0); + }; + + if (last_joy_msg.buttons[BUTTON_ADJUST]) { + static bool is_pushed = false; + if (last_joy_msg.buttons[BUTTON_ADJUST_UP]) { + // trigger button up + if (!is_pushed) { + if (last_joy_msg.buttons[BUTTON_ADJUST_KICK]) { + adjust_value(kick_power, 0.1); + std::cout << "kick up: " << kick_power << std::endl; + } + + if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { + adjust_value(dribble_power, 0.1); + std::cout << "dribble up:" << dribble_power << std::endl; + } + } + is_pushed = true; + } else if (last_joy_msg.buttons[BUTTON_ADJUST_DOWN]) { + // trigger button up + if (!is_pushed) { + if (last_joy_msg.buttons[BUTTON_ADJUST_KICK]) { + adjust_value(kick_power, -0.1); + std::cout << "kick down: " << kick_power << std::endl; + } + if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { + adjust_value(dribble_power, -0.1); + std::cout << "dribble down: " << dribble_power << std::endl; + } + } + is_pushed = true; + } else { + is_pushed = false; + } + } + + Point target = [&]() -> Point { + using boost::math::constants::degree; + double rotation_angle = getParameter("rotation_deg") * degree(); + if (getParameter("use_local_coordinate")) { + rotation_angle += robot()->pose.theta; + } + Eigen::Rotation2Dd rotation(rotation_angle); + return robot()->pose.pos + + rotation.toRotationMatrix() * Point{ + last_joy_msg.axes[AXIS_VEL_SURGE] * MAX_VEL_SURGE, + last_joy_msg.axes[AXIS_VEL_SWAY] * MAX_VEL_SWAY}; + }(); + + command.setTargetPosition(target); + + double angular = + (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_R]) - (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_L]); + + theta += angular; + command.setTargetTheta(normalizeAngle(theta)); + + if (is_kick_enable) { + if (is_kick_mode_straight) { + command.kickStraight(kick_power); + } else { + command.kickWithChip(kick_power); + } + } else { + command.kickStraight(0.0); + } + + if (is_dribble_enable) { + command.dribble(dribble_power); + } else { + command.dribble(0.0); + } + + return Status::RUNNING; +} +} // namespace crane::skills