From 6b8b157c00085acef87446dccf7b2d0602210c9f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 10 Jan 2025 12:30:56 +0900 Subject: [PATCH] =?UTF-8?q?STOP=E4=B8=AD=E3=81=AE=E8=AA=BF=E6=95=B4=20(#67?= =?UTF-8?q?9)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../kick_event_detector.hpp | 4 --- crane_local_planner/src/rvo2_planner.cpp | 13 +++++---- crane_play_switcher/src/play_switcher.cpp | 1 + .../visualization_data_handler.hpp | 4 +++ .../src/visualization_data_handler.cpp | 29 ++++++++++++------- .../src/world_model_publisher.cpp | 2 +- .../src/skill_planners.cpp | 2 +- 7 files changed, 33 insertions(+), 22 deletions(-) diff --git a/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp b/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp index 27a36132b..2eae34b28 100644 --- a/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp +++ b/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp @@ -92,10 +92,6 @@ class KickEventDetector // for (const auto & [kick_origin, kick_end] : kick_history) { // visualizer->addLine(kick_origin.position, kick_end, 2, "red", 0.5, "KICK"); // } - - for (const auto & record : records) { - visualizer->addCircle(record.position, 0.1, 2, "red", "", 1.0, "ball"); - } } bool hasInterruptedOnGoingKick(const WorldModelWrapper & world_model) const diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index b0d1a5027..94a5358e0 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -7,6 +7,7 @@ #include "crane_local_planner/rvo2_planner.hpp" #include +#include // cspell: ignore OBST @@ -60,7 +61,9 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node) void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg) { - if (world_model->play_situation.getSituationCommandID() == crane_msgs::msg::PlaySituation::STOP) { + if ( + world_model->play_situation.getRefereeCommandID() == + robocup_ssl_msgs::msg::Referee::COMMAND_STOP) { // 1.5m/sだとたまに超えるので1.0m/sにしておく for (int i = 0; i < 40; i++) { rvo_sim->setAgentMaxSpeed(i, 1.0f); @@ -146,8 +149,8 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms max_vel = std::min(max_vel, max_vel_by_decel); max_vel = std::min(max_vel, max_vel_by_acc); if ( - world_model->play_situation.getSituationCommandID() == - crane_msgs::msg::PlaySituation::STOP) { + world_model->play_situation.getRefereeCommandID() == + robocup_ssl_msgs::msg::Referee::COMMAND_STOP) { // 1.5m/sだとたまに超えるので1.0m/sにしておく max_vel = std::min(max_vel, 1.0); } @@ -291,8 +294,8 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg) double SURROUNDING_OFFSET = 0.3; double PENALTY_AREA_OFFSET = 0.1; if ( - world_model->play_situation.getSituationCommandID() == - crane_msgs::msg::PlaySituation::STOP) { + world_model->play_situation.getRefereeCommandID() == + robocup_ssl_msgs::msg::Referee::COMMAND_STOP) { PENALTY_AREA_OFFSET = 0.5; SURROUNDING_OFFSET = 0.6; } diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp index 2de2c18a2..bc4d4d113 100644 --- a/crane_play_switcher/src/play_switcher.cpp +++ b/crane_play_switcher/src/play_switcher.cpp @@ -81,6 +81,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg) // TODO(HansRobo): robocup_ssl_msgs/msg/Refereeをもう少しわかりやすい形式にする必要あり play_situation_msg.stage = msg.stage; + play_situation_msg.command_raw = msg.command; if (latest_raw_referee_command != static_cast(msg.command)) { //-----------------------------------// diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp index cd5dd247b..2f70440f9 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp @@ -43,6 +43,10 @@ class VisualizationDataHandler rclcpp::Subscription::SharedPtr sub_referee_; rclcpp::Publisher::SharedPtr pub_vis_objects_; + + double ball_x; + + double ball_y; }; } // namespace crane diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp index 84f1ae964..58723eda4 100644 --- a/crane_world_model_publisher/src/visualization_data_handler.cpp +++ b/crane_world_model_publisher/src/visualization_data_handler.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace crane @@ -33,6 +34,7 @@ using VisPoint = crane_visualization_interfaces::msg::ShapePoint; using VisRect = crane_visualization_interfaces::msg::ShapeRectangle; using VisRobot = crane_visualization_interfaces::msg::ShapeRobot; using VisText = crane_visualization_interfaces::msg::ShapeText; +using VisTube = crane_visualization_interfaces::msg::ShapeTube; using RobotId = robocup_ssl_msgs::msg::RobotId; VisualizationDataHandler::VisualizationDataHandler(rclcpp::Node & node) @@ -154,6 +156,9 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ vis_ball.caption = "ball is here"; vis_objects.circles.push_back(vis_ball); + ball_x = ball.pos().x(); + ball_y = ball.pos().y(); + // 速度を描画 if (ball.has_vel()) { const double vel_norm = std::hypot(ball.vel().x(), ball.vel().y()); @@ -527,17 +532,19 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) msg->command == Referee::COMMAND_BALL_PLACEMENT_BLUE || msg->command == Referee::COMMAND_BALL_PLACEMENT_YELLOW) { if (not msg->designated_position.empty()) { - VisCircle vis_circle; - vis_circle.center.x = msg->designated_position.front().x; - vis_circle.center.y = msg->designated_position.front().y; - vis_circle.radius = 0.15; - vis_circle.line_color.name = "aquamarine"; - vis_circle.line_color.alpha = 1.0; - vis_circle.fill_color.name = "aquamarine"; - vis_circle.fill_color.alpha = 1.0; - vis_circle.line_size = 1; - vis_circle.caption = "placement pos"; - vis_objects.circles.push_back(vis_circle); + VisTube vis_tube; + vis_tube.p1.x = msg->designated_position.front().x / 1000.; + vis_tube.p1.y = msg->designated_position.front().y / 1000.; + vis_tube.p2.x = ball_x; + vis_tube.p2.y = ball_y; + vis_tube.radius = 0.5; + vis_tube.line_color.name = "aquamarine"; + vis_tube.line_color.alpha = 1.0; + // vis_tube.fill_color.name = "aquamarine"; + vis_tube.fill_color.alpha = 0.0; + vis_tube.line_size = 1; + vis_tube.caption = "placement"; + vis_objects.tubes.push_back(vis_tube); } } diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 4f6020e4f..324dadcbd 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -416,7 +416,7 @@ void WorldModelPublisherComponent::publishWorldModel() pub_world_model->publish(wm); - constexpr int SAMPLING_NUM = 2; + constexpr int SAMPLING_NUM = 4; for (const auto & history : friend_history) { if (history.size() > SAMPLING_NUM + 1) { for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) { diff --git a/session/crane_planner_plugins/src/skill_planners.cpp b/session/crane_planner_plugins/src/skill_planners.cpp index 4d5e2f60b..72c07503c 100644 --- a/session/crane_planner_plugins/src/skill_planners.cpp +++ b/session/crane_planner_plugins/src/skill_planners.cpp @@ -269,7 +269,7 @@ auto BallNearByPositionerSkillPlanner::getSelectedRobots( skills.back()->setParameter("line_policy", std::string("arc")); skills.back()->setParameter("positioning_policy", std::string("goal")); skills.back()->setParameter("robot_interval", 0.35); - skills.back()->setParameter("margin_distance", 0.35); + skills.back()->setParameter("margin_distance", 0.6); } return selected;