diff --git a/crane_msgs/msg/control/RobotCommands.msg b/crane_msgs/msg/control/RobotCommands.msg index 6af4cd091..59d71a996 100755 --- a/crane_msgs/msg/control/RobotCommands.msg +++ b/crane_msgs/msg/control/RobotCommands.msg @@ -1,3 +1,4 @@ std_msgs/Header header +bool on_positive_half bool is_yellow RobotCommand[] robot_commands diff --git a/crane_msgs/msg/world_model/WorldModel.msg b/crane_msgs/msg/world_model/WorldModel.msg index 006aa60ed..192c6f17c 100755 --- a/crane_msgs/msg/world_model/WorldModel.msg +++ b/crane_msgs/msg/world_model/WorldModel.msg @@ -1,5 +1,6 @@ std_msgs/Header header +bool on_positive_half bool is_yellow uint8 our_goalie_id @@ -14,6 +15,4 @@ BallInfo ball_info RobotInfoOurs[] robot_info_ours RobotInfoTheirs[] robot_info_theirs -geometry_msgs/Point ball_placement_target - PlaySituation play_situation diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp index 372de6dc3..5479877e3 100644 --- a/crane_play_switcher/src/play_switcher.cpp +++ b/crane_play_switcher/src/play_switcher.cpp @@ -187,6 +187,11 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg) last_command_changed_state.stamp = now(); last_command_changed_state.ball_position = world_model->ball.pos; + if (msg.designated_position.size() > 0) { + play_situation_msg.placement_position.x = msg.designated_position[0].x; + play_situation_msg.placement_position.y = msg.designated_position[0].y; + } + // パブリッシュはコマンド更新時のみ play_situation_pub->publish(play_situation_msg); } diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index 6b3b30e0c..0c08a342e 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -113,7 +113,7 @@ class IbisSenderNode : public SenderBase for (auto command : msg.robot_commands) { // - if (msg.is_yellow) { + if (not msg.on_positive_half) { command.target_velocity.x *= -1; command.target_velocity.y *= -1; command.target_velocity.theta *= -1; diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index 092a5634d..023815fb7 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -110,6 +110,7 @@ class ROSNode : public rclcpp::Node crane_msgs::msg::RobotCommands msg; msg.header = world_model->getMsg().header; msg.is_yellow = world_model->isYellow(); + msg.on_positive_half = world_model->onPositiveHalf(); msg.robot_commands.push_back(latest_msg); publisher_robot_commands->publish(msg); }); diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp index 6fc4edc8f..ad3ecd431 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp @@ -91,6 +91,8 @@ class WorldModelPublisherComponent : public rclcpp::Node Color their_color; + bool on_positive_half; + uint8_t our_goalie_id, their_goalie_id; uint8_t max_id; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 287f1732b..ced6d5367 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -38,6 +38,16 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt declare_parameter("team_name", "ibis-ssl"); team_name = get_parameter("team_name").as_string(); + declare_parameter("initial_team_color", "BLUE"); + auto initial_team_color = get_parameter("initial_team_color").as_string(); + if (initial_team_color == "BLUE") { + our_color = Color::BLUE; + their_color = Color::YELLOW; + } else { + our_color = Color::YELLOW; + their_color = Color::BLUE; + } + sub_referee = this->create_subscription( "/referee", 1, [this](const robocup_ssl_msgs::msg::Referee & msg) { if (msg.yellow.name == team_name) { @@ -46,12 +56,18 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt their_color = Color::BLUE; our_goalie_id = msg.yellow.goalkeeper; their_goalie_id = msg.blue.goalkeeper; + if (not msg.blue_team_on_positive_half.empty()) { + on_positive_half = not msg.blue_team_on_positive_half[0]; + } } else if (msg.blue.name == team_name) { // BLUE our_color = Color::BLUE; their_color = Color::YELLOW; our_goalie_id = msg.blue.goalkeeper; their_goalie_id = msg.yellow.goalkeeper; + if (not msg.blue_team_on_positive_half.empty()) { + on_positive_half = msg.blue_team_on_positive_half[0]; + } } else { std::stringstream what; what << "Cannot find our team name, " << team_name << " in referee message. "; @@ -64,17 +80,6 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt ball_placement_target_y = msg.designated_position.front().y / 1000.; } }); - - declare_parameter("initial_team_color", "BLUE"); - auto initial_team_color = get_parameter("initial_team_color").as_string(); - if (initial_team_color == "BLUE") { - our_color = Color::BLUE; - their_color = Color::YELLOW; - - } else { - our_color = Color::YELLOW; - their_color = Color::BLUE; - } } void WorldModelPublisherComponent::visionDetectionsCallback( @@ -164,6 +169,7 @@ void WorldModelPublisherComponent::publishWorldModel() crane_msgs::msg::WorldModel wm; wm.is_yellow = (our_color == Color::YELLOW); + wm.on_positive_half = on_positive_half; wm.ball_info = ball_info; updateBallContact(); @@ -196,9 +202,6 @@ void WorldModelPublisherComponent::publishWorldModel() wm.goal_size.x = goal_h; wm.goal_size.y = goal_w; - wm.ball_placement_target.x = ball_placement_target_x; - wm.ball_placement_target.y = ball_placement_target_y; - wm.our_goalie_id = our_goalie_id; wm.their_goalie_id = their_goalie_id; diff --git a/session/crane_planner_base/include/crane_planner_base/planner_base.hpp b/session/crane_planner_base/include/crane_planner_base/planner_base.hpp index db2257e97..f211c4c92 100644 --- a/session/crane_planner_base/include/crane_planner_base/planner_base.hpp +++ b/session/crane_planner_base/include/crane_planner_base/planner_base.hpp @@ -69,7 +69,8 @@ class PlannerBase status = latest_status; crane_msgs::msg::RobotCommands msg; msg.is_yellow = world_model->isYellow(); - for (auto command : robot_commands) { + msg.on_positive_half = world_model->onPositiveHalf(); + for (const auto & command : robot_commands) { msg.robot_commands.emplace_back(command); } return msg; diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index f93bb069f..5ba34c8f6 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -142,6 +142,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions world_model->addCallback([this]() { crane_msgs::msg::RobotCommands msg; msg.header = world_model->getMsg().header; + msg.on_positive_half = world_model->onPositiveHalf(); msg.is_yellow = world_model->isYellow(); for (const auto & planner : available_planners) { auto commands_msg = planner->getRobotCommands(); diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 90aed1824..cbb405c6b 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -257,9 +257,9 @@ struct WorldModelWrapper defense_area_size << world_model.defense_area_size.x, world_model.defense_area_size.y; goal_size << world_model.goal_size.x, world_model.goal_size.y; - goal << (isYellow() ? field_size.x() * 0.5 : -field_size.x() * 0.5), 0.; + goal << getOurSideSign() * field_size.x() * 0.5, 0.; - if (goal.x() > 0) { + if (onPositiveHalf()) { ours.defense_area.max_corner() << goal.x(), goal.y() + world_model.defense_area_size.y / 2.; ours.defense_area.min_corner() << goal.x() - world_model.defense_area_size.x, goal.y() - world_model.defense_area_size.y / 2.; @@ -274,19 +274,14 @@ struct WorldModelWrapper theirs.defense_area.min_corner() << std::min(-ours.defense_area.max_corner().x(), -ours.defense_area.min_corner().x()), ours.defense_area.min_corner().y(); - - if ( - world_model.play_situation.command == crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or - world_model.play_situation.command == crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT) { - *ball_placement_target << world_model.ball_placement_target.x, - world_model.ball_placement_target.y; - } else { - ball_placement_target = std::nullopt; - } } [[nodiscard]] const crane_msgs::msg::WorldModel & getMsg() const { return latest_msg; } + [[nodiscard]] bool onPositiveHalf() const { return (latest_msg.on_positive_half); } + + [[nodiscard]] double getOurSideSign() const { return onPositiveHalf() ? 1.0 : -1.0; } + [[nodiscard]] bool isYellow() const { return (latest_msg.is_yellow); } [[nodiscard]] bool hasUpdated() const { return has_updated; } @@ -456,16 +451,24 @@ struct WorldModelWrapper [[nodiscard]] std::optional getBallPlacementTarget() const { - return ball_placement_target; + if ( + play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or + play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT) { + return play_situation.placement_position; + } else { + return std::nullopt; + } } // rule 8.4.3 [[nodiscard]] std::optional getBallPlacementArea() const { - if (ball_placement_target) { + if (auto target = getBallPlacementTarget()) { Capsule area; area.segment.first = ball.pos; - area.segment.second = ball_placement_target.value(); + area.segment.second = target.value(); area.radius = 0.5; return area; } else { diff --git a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp index e450f06a5..d3b98bbbd 100644 --- a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp @@ -66,7 +66,7 @@ auto PlaySituationWrapper::update(const crane_msgs::msg::PlaySituation & msg) -> situation_command.id = msg.command; situation_command.text = situation_command_map[msg.command]; - placement_position << msg.placement_position.x, msg.placement_position.y; + placement_position << msg.placement_position.x / 1000., msg.placement_position.y / 1000.; } auto PlaySituationWrapper::getRefereeCommandText(uint32_t id) -> std::string