From 267a5babe66e07b9608bf9941a837af05ed8df46 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:14:34 +0900 Subject: [PATCH 01/12] =?UTF-8?q?control=5Fmode=E3=81=AE=E3=83=87=E3=83=95?= =?UTF-8?q?=E3=82=A9=E3=83=AB=E3=83=88=E3=82=92=E4=BD=8D=E7=BD=AE=E3=83=A2?= =?UTF-8?q?=E3=83=BC=E3=83=89=E3=81=AB?= 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 4da90e6b6..02e086f06 100755 --- a/crane_msgs/msg/control/RobotCommand.msg +++ b/crane_msgs/msg/control/RobotCommand.msg @@ -39,7 +39,7 @@ uint8 POSITION_TARGET_MODE=1 uint8 SIMPLE_VELOCITY_TARGET_MODE=2 uint8 VELOCITY_TARGET_MODE=3 -uint8 control_mode +uint8 control_mode 1 string skill_name From 611ca4ca2680e7ec96a4f1ffcc5a7b2cc58cc6ef Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:14:55 +0900 Subject: [PATCH 02/12] =?UTF-8?q?GoalKick=E3=82=AF=E3=83=A9=E3=82=B9?= =?UTF-8?q?=E3=81=AE=E5=9B=9E=E3=82=8A=E8=BE=BC=E3=81=BF=E8=B7=9D=E9=9B=A2?= =?UTF-8?q?=E3=82=92=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/include/crane_robot_skills/goal_kick.hpp | 1 + 1 file changed, 1 insertion(+) 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 276b85316..0ed7ef065 100644 --- a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp @@ -26,6 +26,7 @@ class GoalKick : public SkillBase kick_skill.setParameter("chip_kick", false); kick_skill.setParameter("with_dribble", false); kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); + kick_skill.setParameter("around_interval", 0.25f); } Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override From 96de4cf3d7ca87d173bee93829ab60bdc000180b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:15:12 +0900 Subject: [PATCH 03/12] =?UTF-8?q?=E3=82=AD=E3=83=BC=E3=83=91=E3=83=BC?= =?UTF-8?q?=E3=81=AE=E5=BE=85=E6=A9=9F=E8=B7=9D=E9=9B=A2=E3=82=92=E5=A4=89?= =?UTF-8?q?=E6=9B=B4?= 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 60e3883e3..33a938f35 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -14,7 +14,7 @@ Goalie::Goalie(RobotCommandWrapperBase::SharedPtr & base) kick_skill(base) { setParameter("run_inplay", true); - setParameter("block_distance", 1.0); + setParameter("block_distance", 0.3); } Status Goalie::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) From 40db4007118c01fc7761955adb5d8798640d0189 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:15:26 +0900 Subject: [PATCH 04/12] =?UTF-8?q?sender=E3=81=8C=E8=90=BD=E3=81=A1?= =?UTF-8?q?=E3=82=8B=E3=83=90=E3=82=B0=E3=82=92=E3=81=84=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_sender/src/ibis_sender_node.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index bdbd5ff78..ec6e77450 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -175,8 +175,13 @@ class IbisSenderNode : public SenderBase packet.prioritize_move = true; packet.prioritize_accurate_acceleration = true; - auto elapsed_time = now - world_model->getOurRobot(command.robot_id)->detection_stamp; - packet.elapsed_time_ms_since_last_vision = elapsed_time.nanoseconds() / 1e6; + try { + auto elapsed_time = now - world_model->getOurRobot(command.robot_id)->detection_stamp; + packet.elapsed_time_ms_since_last_vision = elapsed_time.nanoseconds() / 1e6; + }catch (...){ + std::cerr << "Error: Failed to get elapsed time of vision from world_model" << std::endl; + packet.elapsed_time_ms_since_last_vision = 0.0; + } switch (command.control_mode) { case crane_msgs::msg::RobotCommand::POSITION_TARGET_MODE: { From 3541ed00f55209d5af9f80e005d70ba95c0c705b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:15:40 +0900 Subject: [PATCH 05/12] =?UTF-8?q?MFT=E7=94=A8=E3=81=AElaunch=E3=83=95?= =?UTF-8?q?=E3=82=A1=E3=82=A4=E3=83=AB=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/mft.launch.xml | 142 ++++++++++++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 crane_bringup/launch/mft.launch.xml diff --git a/crane_bringup/launch/mft.launch.xml b/crane_bringup/launch/mft.launch.xml new file mode 100644 index 000000000..ad7fc6fc3 --- /dev/null +++ b/crane_bringup/launch/mft.launch.xml @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From dd08f8f4c62c3edbedf791774bb4aad8b3c32f08 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:16:05 +0900 Subject: [PATCH 06/12] =?UTF-8?q?dribble=E6=99=82=E3=81=AB=E5=BC=B7?= =?UTF-8?q?=E5=88=B6=E7=9A=84=E3=81=AB=E3=82=AD=E3=83=83=E3=82=AF=E3=83=91?= =?UTF-8?q?=E3=83=AF=E3=83=BC=E3=81=AE=E3=83=AA=E3=82=BB=E3=83=83=E3=83=88?= =?UTF-8?q?=E3=82=92=E3=81=97=E3=81=AA=E3=81=84=E3=82=88=E3=81=86=E3=81=AB?= =?UTF-8?q?=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_msg_wrappers/robot_command_wrapper.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp index d8ac7fc44..b5b41a6b5 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp @@ -97,7 +97,7 @@ class RobotCommandWrapperCommon T & dribble(double power) { command->latest_msg.dribble_power = power; - command->latest_msg.kick_power = 0.0; +// command->latest_msg.kick_power = 0.0; return static_cast(*this); } From 34b874495abbb9ecf6d9906919fa826056fe1332 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:16:21 +0900 Subject: [PATCH 07/12] =?UTF-8?q?=E7=8B=AD=E3=81=84=E3=81=AE=E3=81=A7?= =?UTF-8?q?=E3=83=AB=E3=83=BC=E3=83=AB=E7=84=A1=E8=A6=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/src/gridmap_planner.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/crane_local_planner/src/gridmap_planner.cpp b/crane_local_planner/src/gridmap_planner.cpp index 07877e48b..89dd0da35 100644 --- a/crane_local_planner/src/gridmap_planner.cpp +++ b/crane_local_planner/src/gridmap_planner.cpp @@ -199,9 +199,10 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( case crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE: case crane_msgs::msg::PlaySituation::OUR_INDIRECT_FREE: // ほんとうは0.2mだがバッファを0.2mとる - return 0.2 + 0.2; +// return 0.2 + 0.2; + return 0.0; default: - return 0.1; + return 0.0; } }(); @@ -236,7 +237,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( } map["friend_robot"].setZero(); for (const auto & robot : world_model->ours.getAvailableRobots()) { - for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.2); !iterator.isPastEnd(); + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); ++iterator) { map.at("friend_robot", *iterator) = 1.0; } @@ -248,7 +249,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( } map["enemy_robot"].setZero(); for (const auto & robot : world_model->theirs.getAvailableRobots()) { - for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.2); !iterator.isPastEnd(); + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); ++iterator) { map.at("enemy_robot", *iterator) = 1.0; } @@ -272,7 +273,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( switch (world_model->play_situation.getSituationCommandID()) { case crane_msgs::msg::PlaySituation::STOP: { // 5.1.1 ボールと0.5m以上離れる必要がある - for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.6); + for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.2); !iterator.isPastEnd(); ++iterator) { map.at("rule", *iterator) = 1.0; } @@ -282,7 +283,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( grid_map::Position position; for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { map.getPosition(*iterator, position); - map.at("rule", *iterator) = world_model->point_checker.isBallPlacementArea(position, 0.2); + map.at("rule", *iterator) = world_model->point_checker.isBallPlacementArea(position, -0.2); } } break; // case crane_msgs::msg::PlaySituation::THEIR_KICKOFF_PREPARATION: From 39a2af3ffc244c18f318b5473e9e5e188c962122 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 21 Sep 2024 18:16:30 +0900 Subject: [PATCH 08/12] =?UTF-8?q?teleop=E6=94=B9=E8=89=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/teleop.hpp | 139 ++++++++++-------- 1 file changed, 80 insertions(+), 59 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/teleop.hpp b/crane_robot_skills/include/crane_robot_skills/teleop.hpp index 47135b1b0..a56bce80f 100644 --- a/crane_robot_skills/include/crane_robot_skills/teleop.hpp +++ b/crane_robot_skills/include/crane_robot_skills/teleop.hpp @@ -44,20 +44,23 @@ class Teleop : public SkillBase, public rclcpp::Nod const int AXIS_VEL_SURGE = 1; const int AXIS_VEL_SWAY = 0; + const int AXIS_ANGULAR_X = 3; + const int AXIS_ANGULAR_Y = 4; 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_KICK_TOGGLE = 1; // O +// const int BUTTON_KICK_STRAIGHT = 13; +// const int BUTTON_KICK_CHIP = 14; +// const int BUTTON_ADJUST_KICK = 1; // O - const int BUTTON_ADJUST = 10; - const int BUTTON_ADJUST_UP = 11; - const int BUTTON_ADJUST_DOWN = 12; +// const int BUTTON_ADJUST = 10; + // select +// const int BUTTON_ADJUST_UP = 8; +// const int BUTTON_ADJUST_DOWN = 9; - const int BUTTON_DRIBBLE_TOGGLE = 6; - const int BUTTON_ADJUST_DRIBBLE = 1; + const int BUTTON_DRIBBLE_TOGGLE = 0; // X +// const int BUTTON_ADJUST_DRIBBLE = 1; const double MAX_VEL_SURGE = 1.0; const double MAX_VEL_SWAY = 1.0; @@ -67,14 +70,16 @@ class Teleop : public SkillBase, public rclcpp::Nod 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; - } + is_kick_mode_straight = true; +// is_kick_enable = true; +// 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 @@ -95,48 +100,50 @@ class Teleop : public SkillBase, public rclcpp::Nod 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; - } - } - +// 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); +// kick_power = 0.15; +// std::cout << "kick up: " << kick_power << std::endl; +// } +// +// if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { +//// adjust_value(dribble_power, 0.1); +// dribble_power = 0.3; +// 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; +// } +// } + + using boost::math::constants::degree; + double rotation_angle = getParameter("rotation_deg") * degree(); 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; } @@ -152,9 +159,16 @@ class Teleop : public SkillBase, public rclcpp::Nod double angular = (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_R]) - (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_L]); - theta += angular; + Vector2 angle; + angle << last_joy_msg.axes[AXIS_ANGULAR_X], -last_joy_msg.axes[AXIS_ANGULAR_Y]; + if(angle.norm() > 0.3) { + theta = getAngle(angle) + rotation_angle + M_PI / 2.0; + } +// theta += angular * 0.1; command.setTargetTheta(normalizeAngle(theta)); + is_kick_mode_straight = true; + kick_power = 0.25; if (is_kick_enable) { if (is_kick_mode_straight) { command.kickStraight(kick_power); @@ -165,11 +179,18 @@ class Teleop : public SkillBase, public rclcpp::Nod command.kickStraight(0.0); } + dribble_power = 0.5; if (is_dribble_enable) { + command.kickStraight(0.0); command.dribble(dribble_power); } else { command.dribble(0.0); } + command.disableBallAvoidance(); + command.disableCollisionAvoidance(); + command.disableGoalAreaAvoidance(); + command.disablePlacementAvoidance(); + command.disableRuleAreaAvoidance(); return Status::RUNNING; } From d536fad8f4add7857be3e6495205f31beb53df17 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 22 Sep 2024 00:41:56 +0900 Subject: [PATCH 09/12] pre-commit --- crane_bringup/launch/crane.launch.xml | 2 +- crane_bringup/launch/mft.launch.xml | 24 ++-- crane_local_planner/src/gridmap_planner.cpp | 2 +- .../include/crane_robot_skills/teleop.hpp | 116 +++++++++--------- crane_sender/src/ibis_sender_node.cpp | 2 +- docker/docker-compose.yaml | 48 ++++---- .../robot_command_wrapper.hpp | 2 +- 7 files changed, 98 insertions(+), 98 deletions(-) diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml index f545ae38d..601675b9e 100644 --- a/crane_bringup/launch/crane.launch.xml +++ b/crane_bringup/launch/crane.launch.xml @@ -92,7 +92,7 @@ - + diff --git a/crane_bringup/launch/mft.launch.xml b/crane_bringup/launch/mft.launch.xml index ad7fc6fc3..f12a91217 100644 --- a/crane_bringup/launch/mft.launch.xml +++ b/crane_bringup/launch/mft.launch.xml @@ -3,12 +3,12 @@ - + - + @@ -83,16 +83,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/crane_local_planner/src/gridmap_planner.cpp b/crane_local_planner/src/gridmap_planner.cpp index 89dd0da35..ce16cb6a8 100644 --- a/crane_local_planner/src/gridmap_planner.cpp +++ b/crane_local_planner/src/gridmap_planner.cpp @@ -199,7 +199,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( case crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE: case crane_msgs::msg::PlaySituation::OUR_INDIRECT_FREE: // ほんとうは0.2mだがバッファを0.2mとる -// return 0.2 + 0.2; + // return 0.2 + 0.2; return 0.0; default: return 0.0; diff --git a/crane_robot_skills/include/crane_robot_skills/teleop.hpp b/crane_robot_skills/include/crane_robot_skills/teleop.hpp index a56bce80f..a461c8169 100644 --- a/crane_robot_skills/include/crane_robot_skills/teleop.hpp +++ b/crane_robot_skills/include/crane_robot_skills/teleop.hpp @@ -50,17 +50,17 @@ class Teleop : public SkillBase, public rclcpp::Nod const int AXIS_VEL_ANGULAR_L = 5; const int BUTTON_KICK_TOGGLE = 1; // O -// const int BUTTON_KICK_STRAIGHT = 13; -// const int BUTTON_KICK_CHIP = 14; -// const int BUTTON_ADJUST_KICK = 1; // O + // const int BUTTON_KICK_STRAIGHT = 13; + // const int BUTTON_KICK_CHIP = 14; + // const int BUTTON_ADJUST_KICK = 1; // O -// const int BUTTON_ADJUST = 10; + // const int BUTTON_ADJUST = 10; // select -// const int BUTTON_ADJUST_UP = 8; -// const int BUTTON_ADJUST_DOWN = 9; + // const int BUTTON_ADJUST_UP = 8; + // const int BUTTON_ADJUST_DOWN = 9; const int BUTTON_DRIBBLE_TOGGLE = 0; // X -// const int BUTTON_ADJUST_DRIBBLE = 1; + // const int BUTTON_ADJUST_DRIBBLE = 1; const double MAX_VEL_SURGE = 1.0; const double MAX_VEL_SWAY = 1.0; @@ -71,15 +71,15 @@ class Teleop : public SkillBase, public rclcpp::Nod static bool is_dribble_enable = false; is_kick_mode_straight = true; -// is_kick_enable = true; -// 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; -// } + // is_kick_enable = true; + // 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 @@ -100,46 +100,46 @@ class Teleop : public SkillBase, public rclcpp::Nod 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); -// kick_power = 0.15; -// std::cout << "kick up: " << kick_power << std::endl; -// } -// -// if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { -//// adjust_value(dribble_power, 0.1); -// dribble_power = 0.3; -// 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; -// } -// } + // 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); + // kick_power = 0.15; + // std::cout << "kick up: " << kick_power << std::endl; + // } + // + // if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { + //// adjust_value(dribble_power, 0.1); + // dribble_power = 0.3; + // 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; + // } + // } using boost::math::constants::degree; double rotation_angle = getParameter("rotation_deg") * degree(); @@ -161,10 +161,10 @@ class Teleop : public SkillBase, public rclcpp::Nod Vector2 angle; angle << last_joy_msg.axes[AXIS_ANGULAR_X], -last_joy_msg.axes[AXIS_ANGULAR_Y]; - if(angle.norm() > 0.3) { + if (angle.norm() > 0.3) { theta = getAngle(angle) + rotation_angle + M_PI / 2.0; } -// theta += angular * 0.1; + // theta += angular * 0.1; command.setTargetTheta(normalizeAngle(theta)); is_kick_mode_straight = true; diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index ec6e77450..08f6ea577 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -178,7 +178,7 @@ class IbisSenderNode : public SenderBase try { auto elapsed_time = now - world_model->getOurRobot(command.robot_id)->detection_stamp; packet.elapsed_time_ms_since_last_vision = elapsed_time.nanoseconds() / 1e6; - }catch (...){ + } catch (...) { std::cerr << "Error: Failed to get elapsed time of vision from world_model" << std::endl; packet.elapsed_time_ms_since_last_vision = 0.0; } diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 654d201e3..ec1462680 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -40,31 +40,31 @@ services: # ports: # - 8083:8083/tcp # - ssl-remote-control: - image: robocupssl/ssl-remote-control:0.3.2 - command: - - -address - - :8084 - - -refereeAddress - - 224.5.23.1:11111 - - -team - - YELLOW - network_mode: host - ports: - - 8084:8084/tcp +# ssl-remote-control: +# image: robocupssl/ssl-remote-control:0.3.2 +# command: +# - -address +# - :8084 +# - -refereeAddress +# - 224.5.23.1:11111 +# - -team +# - YELLOW +# network_mode: host +# ports: +# - 8084:8084/tcp - autoref-tigers: - image: tigersmannheim/auto-referee:1.2.0 - command: - - -a # active mode (connect to GC) - - -hl # headless - - --visionAddress - - 224.5.23.2:10006 - - --refereeAddress - - 224.5.23.1:11111 - - --trackerAddress - - 224.5.23.2:11010 - network_mode: host +# autoref-tigers: +# image: tigersmannheim/auto-referee:1.2.0 +# command: +# - -a # active mode (connect to GC) +# - -hl # headless +# - --visionAddress +# - 224.5.23.2:10006 +# - --refereeAddress +# - 224.5.23.1:11111 +# - --trackerAddress +# - 224.5.23.2:11010 +# network_mode: host # autoref-erforce: # image: roboticserlangen/autoref:commit-6f15f574ea80 diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp index b5b41a6b5..268ce03d2 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp @@ -97,7 +97,7 @@ class RobotCommandWrapperCommon T & dribble(double power) { command->latest_msg.dribble_power = power; -// command->latest_msg.kick_power = 0.0; + // command->latest_msg.kick_power = 0.0; return static_cast(*this); } From 4c2f816d8f612979059f64dc1f56675a56b93094 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 11 Oct 2024 21:01:06 +0900 Subject: [PATCH 10/12] =?UTF-8?q?teleop=E6=94=B9=E8=89=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/teleop.hpp | 177 +--------------- crane_robot_skills/src/teleop.cpp | 192 ++++++++++++++++++ 2 files changed, 194 insertions(+), 175 deletions(-) create mode 100644 crane_robot_skills/src/teleop.cpp diff --git a/crane_robot_skills/include/crane_robot_skills/teleop.hpp b/crane_robot_skills/include/crane_robot_skills/teleop.hpp index a461c8169..2f656cf1c 100644 --- a/crane_robot_skills/include/crane_robot_skills/teleop.hpp +++ b/crane_robot_skills/include/crane_robot_skills/teleop.hpp @@ -18,182 +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_ANGULAR_X = 3; - const int AXIS_ANGULAR_Y = 4; - const int AXIS_VEL_ANGULAR_R = 2; - const int AXIS_VEL_ANGULAR_L = 5; - - const int BUTTON_KICK_TOGGLE = 1; // O - // const int BUTTON_KICK_STRAIGHT = 13; - // const int BUTTON_KICK_CHIP = 14; - // const int BUTTON_ADJUST_KICK = 1; // O - - // const int BUTTON_ADJUST = 10; - // select - // const int BUTTON_ADJUST_UP = 8; - // const int BUTTON_ADJUST_DOWN = 9; - - const int BUTTON_DRIBBLE_TOGGLE = 0; // X - // 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; - - is_kick_mode_straight = true; - // is_kick_enable = true; - // 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); - // kick_power = 0.15; - // std::cout << "kick up: " << kick_power << std::endl; - // } - // - // if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { - //// adjust_value(dribble_power, 0.1); - // dribble_power = 0.3; - // 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; - // } - // } - - using boost::math::constants::degree; - double rotation_angle = getParameter("rotation_deg") * degree(); - Point target = [&]() -> Point { - 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]); - - Vector2 angle; - angle << last_joy_msg.axes[AXIS_ANGULAR_X], -last_joy_msg.axes[AXIS_ANGULAR_Y]; - if (angle.norm() > 0.3) { - theta = getAngle(angle) + rotation_angle + M_PI / 2.0; - } - // theta += angular * 0.1; - command.setTargetTheta(normalizeAngle(theta)); - - is_kick_mode_straight = true; - kick_power = 0.25; - if (is_kick_enable) { - if (is_kick_mode_straight) { - command.kickStraight(kick_power); - } else { - command.kickWithChip(kick_power); - } - } else { - command.kickStraight(0.0); - } - - dribble_power = 0.5; - if (is_dribble_enable) { - command.kickStraight(0.0); - command.dribble(dribble_power); - } else { - command.dribble(0.0); - } - command.disableBallAvoidance(); - command.disableCollisionAvoidance(); - command.disableGoalAreaAvoidance(); - command.disablePlacementAvoidance(); - command.disableRuleAreaAvoidance(); - - 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/teleop.cpp b/crane_robot_skills/src/teleop.cpp new file mode 100644 index 000000000..3d40011d9 --- /dev/null +++ b/crane_robot_skills/src/teleop.cpp @@ -0,0 +1,192 @@ +// 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_ANGULAR_X = 3; + const int AXIS_ANGULAR_Y = 4; + const int AXIS_VEL_ANGULAR_R = 2; + const int AXIS_VEL_ANGULAR_L = 5; + + const int BUTTON_KICK_TOGGLE = 1; // O +// const int BUTTON_KICK_STRAIGHT = 13; +// const int BUTTON_KICK_CHIP = 14; +// const int BUTTON_ADJUST_KICK = 1; // O + +// const int BUTTON_ADJUST = 10; + // select +// const int BUTTON_ADJUST_UP = 8; +// const int BUTTON_ADJUST_DOWN = 9; + + const int BUTTON_DRIBBLE_TOGGLE = 0; // X +// 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; + + is_kick_mode_straight = true; +// is_kick_enable = true; +// 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); +// kick_power = 0.15; +// std::cout << "kick up: " << kick_power << std::endl; +// } +// +// if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { +//// adjust_value(dribble_power, 0.1); +// dribble_power = 0.3; +// 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; +// } +// } + + using boost::math::constants::degree; + double rotation_angle = getParameter("rotation_deg") * degree(); + Point target = [&]() -> Point { + 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]); + + Vector2 angle; + angle << last_joy_msg.axes[AXIS_ANGULAR_X], -last_joy_msg.axes[AXIS_ANGULAR_Y]; + if(angle.norm() > 0.3) { + theta = getAngle(angle) + rotation_angle + M_PI / 2.0; + } +// theta += angular * 0.1; +// command.setTargetTheta(normalizeAngle(theta)); + auto [best_angle, goal_angle_width] = + world_model()->getLargestGoalAngleRangeFromPoint(robot()->pose.pos); +// command.lookAt(world_model()->getTheirGoalCenter()); + command.setTargetTheta(best_angle); + is_kick_enable = true; + is_kick_mode_straight = true; + kick_power = 0.25; + if (is_kick_enable) { + if (is_kick_mode_straight) { + command.kickStraight(kick_power); + } else { + command.kickWithChip(kick_power); + } + } else { + command.kickStraight(0.0); + } + + dribble_power = 0.5; + if (is_dribble_enable) { + command.kickStraight(0.0); + command.dribble(dribble_power); + } else { + command.dribble(0.0); + } + command.disableBallAvoidance(); + command.disableCollisionAvoidance(); + command.disableGoalAreaAvoidance(); + command.disablePlacementAvoidance(); + command.disableRuleAreaAvoidance(); + + return Status::RUNNING; +} +} // namespace crane::skills From e6ea1a4d263669aa241d6205d72c712cae32428f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 11 Oct 2024 21:01:18 +0900 Subject: [PATCH 11/12] =?UTF-8?q?=E6=9C=AA=E3=82=B3=E3=83=9F=E3=83=83?= =?UTF-8?q?=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/mft.launch.xml | 6 ++--- crane_speaker/src/crane_speaker_node.cpp | 29 ++++++++++++------------ 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/crane_bringup/launch/mft.launch.xml b/crane_bringup/launch/mft.launch.xml index f12a91217..48a4ff46d 100644 --- a/crane_bringup/launch/mft.launch.xml +++ b/crane_bringup/launch/mft.launch.xml @@ -129,11 +129,11 @@ - + - + - + diff --git a/crane_speaker/src/crane_speaker_node.cpp b/crane_speaker/src/crane_speaker_node.cpp index 318d7da67..930780650 100644 --- a/crane_speaker/src/crane_speaker_node.cpp +++ b/crane_speaker/src/crane_speaker_node.cpp @@ -13,22 +13,23 @@ typedef std::unordered_map map; map play_situation_map = { - {crane_msgs::msg::PlaySituation::HALT, "ホールト"}, - {crane_msgs::msg::PlaySituation::STOP, "ストップ"}, - {crane_msgs::msg::PlaySituation::OUR_KICKOFF_PREPARATION, "味方キックオフ準備"}, - {crane_msgs::msg::PlaySituation::OUR_KICKOFF_START, "味方キックオフ開始"}, - {crane_msgs::msg::PlaySituation::OUR_PENALTY_PREPARATION, "味方PK準備"}, - {crane_msgs::msg::PlaySituation::OUR_PENALTY_START, "味方PK開始"}, + {crane_msgs::msg::PlaySituation::HALT, "停止します"}, + {crane_msgs::msg::PlaySituation::STOP, "待機します"}, + {crane_msgs::msg::PlaySituation::OUR_KICKOFF_PREPARATION, "味方キックオフを準備します"}, + {crane_msgs::msg::PlaySituation::OUR_KICKOFF_START, "味方キックオフを始めます"}, + {crane_msgs::msg::PlaySituation::OUR_PENALTY_PREPARATION, "味方ペナルティキックを準備します"}, + {crane_msgs::msg::PlaySituation::OUR_PENALTY_START, "味方ペナルティキックを始めます"}, {crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE, "味方フリーキック"}, - {crane_msgs::msg::PlaySituation::OUR_INDIRECT_FREE, "味方インダイレクトフリーキック"}, - {crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT, "味方ボールプレイスメント"}, - {crane_msgs::msg::PlaySituation::THEIR_KICKOFF_PREPARATION, "敵キックオフ準備"}, - {crane_msgs::msg::PlaySituation::THEIR_KICKOFF_START, "敵キックオフ開始"}, - {crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION, "敵PK準備"}, - {crane_msgs::msg::PlaySituation::THEIR_PENALTY_START, "敵PK開始"}, + {crane_msgs::msg::PlaySituation::OUR_INDIRECT_FREE, "味方フリーキック"}, + {crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT, "味方がボールを動かします"}, + {crane_msgs::msg::PlaySituation::THEIR_KICKOFF_PREPARATION, "敵キックオフを準備します"}, + {crane_msgs::msg::PlaySituation::THEIR_KICKOFF_START, "敵キックオフを始めます"}, + {crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION, "敵ペナルティキックを準備します"}, + {crane_msgs::msg::PlaySituation::THEIR_PENALTY_START, "敵ペナルティキックを始めます"}, {crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE, "敵フリーキック"}, - {crane_msgs::msg::PlaySituation::THEIR_INDIRECT_FREE, "敵インダイレクトフリーキック"}, - {crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT, "敵ボールプレイスメント"}, + {crane_msgs::msg::PlaySituation::THEIR_INDIRECT_FREE, "敵フリーキック"}, + {crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT, "敵がボールを動かします"}, + {crane_msgs::msg::PlaySituation::INPLAY, "ゲームを始めます"}, {crane_msgs::msg::PlaySituation::OUR_INPLAY, "味方ボール"}, {crane_msgs::msg::PlaySituation::THEIR_INPLAY, "敵ボール"}, {crane_msgs::msg::PlaySituation::AMBIGUOUS_INPLAY, "フリーボール"}}; From 70cd5caa19bf239cb58561f744cb5819b5c00e4e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 11 Oct 2024 12:04:13 +0000 Subject: [PATCH 12/12] style(pre-commit): autofix --- crane_robot_skills/src/teleop.cpp | 126 +++++++++++++++--------------- 1 file changed, 63 insertions(+), 63 deletions(-) diff --git a/crane_robot_skills/src/teleop.cpp b/crane_robot_skills/src/teleop.cpp index 3d40011d9..8c89e5c84 100644 --- a/crane_robot_skills/src/teleop.cpp +++ b/crane_robot_skills/src/teleop.cpp @@ -10,7 +10,7 @@ namespace crane::skills { Teleop::Teleop(RobotCommandWrapperBase::SharedPtr & base) - : SkillBase("Teleop", base), Node("teleop_skill") +: SkillBase("Teleop", base), Node("teleop_skill") { setParameter("rotation_deg", 0.); setParameter("use_local_coordinate", false); @@ -41,17 +41,17 @@ Status Teleop::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) const int AXIS_VEL_ANGULAR_L = 5; const int BUTTON_KICK_TOGGLE = 1; // O -// const int BUTTON_KICK_STRAIGHT = 13; -// const int BUTTON_KICK_CHIP = 14; -// const int BUTTON_ADJUST_KICK = 1; // O + // const int BUTTON_KICK_STRAIGHT = 13; + // const int BUTTON_KICK_CHIP = 14; + // const int BUTTON_ADJUST_KICK = 1; // O -// const int BUTTON_ADJUST = 10; + // const int BUTTON_ADJUST = 10; // select -// const int BUTTON_ADJUST_UP = 8; -// const int BUTTON_ADJUST_DOWN = 9; + // const int BUTTON_ADJUST_UP = 8; + // const int BUTTON_ADJUST_DOWN = 9; const int BUTTON_DRIBBLE_TOGGLE = 0; // X -// const int BUTTON_ADJUST_DRIBBLE = 1; + // const int BUTTON_ADJUST_DRIBBLE = 1; const double MAX_VEL_SURGE = 1.0; const double MAX_VEL_SWAY = 1.0; @@ -62,15 +62,15 @@ Status Teleop::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) static bool is_dribble_enable = false; is_kick_mode_straight = true; -// is_kick_enable = true; -// 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; -// } + // is_kick_enable = true; + // 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 @@ -91,46 +91,46 @@ Status Teleop::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) 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); -// kick_power = 0.15; -// std::cout << "kick up: " << kick_power << std::endl; -// } -// -// if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { -//// adjust_value(dribble_power, 0.1); -// dribble_power = 0.3; -// 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; -// } -// } + // 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); + // kick_power = 0.15; + // std::cout << "kick up: " << kick_power << std::endl; + // } + // + // if (last_joy_msg.buttons[BUTTON_ADJUST_DRIBBLE]) { + //// adjust_value(dribble_power, 0.1); + // dribble_power = 0.3; + // 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; + // } + // } using boost::math::constants::degree; double rotation_angle = getParameter("rotation_deg") * degree(); @@ -141,8 +141,8 @@ Status Teleop::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) 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}; + last_joy_msg.axes[AXIS_VEL_SURGE] * MAX_VEL_SURGE, + last_joy_msg.axes[AXIS_VEL_SWAY] * MAX_VEL_SWAY}; }(); command.setTargetPosition(target); @@ -152,14 +152,14 @@ Status Teleop::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) Vector2 angle; angle << last_joy_msg.axes[AXIS_ANGULAR_X], -last_joy_msg.axes[AXIS_ANGULAR_Y]; - if(angle.norm() > 0.3) { + if (angle.norm() > 0.3) { theta = getAngle(angle) + rotation_angle + M_PI / 2.0; } -// theta += angular * 0.1; -// command.setTargetTheta(normalizeAngle(theta)); + // theta += angular * 0.1; + // command.setTargetTheta(normalizeAngle(theta)); auto [best_angle, goal_angle_width] = world_model()->getLargestGoalAngleRangeFromPoint(robot()->pose.pos); -// command.lookAt(world_model()->getTheirGoalCenter()); + // command.lookAt(world_model()->getTheirGoalCenter()); command.setTargetTheta(best_angle); is_kick_enable = true; is_kick_mode_straight = true;