diff --git a/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py b/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py index 05810e831..37d0814cc 100644 --- a/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py +++ b/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py @@ -24,7 +24,7 @@ import rclpy from ament_index_python.resources import get_resource -from consai_visualizer_msgs.msg import Objects +from consai_visualizer_msgs.msg import ObjectsArray from consai_visualizer.field_widget import FieldWidget from python_qt_binding import loadUi from python_qt_binding.QtCore import QPointF, Qt, QTimer @@ -64,8 +64,8 @@ def __init__(self, context): self._widget.field_widget.set_logger(self._logger) self._add_visualizer_layer("caption", "caption") - self._sub_visualize_objects = self._node.create_subscription( - Objects, + self._sub_visualize_objects_array = self._node.create_subscription( + ObjectsArray, "visualizer_objects", self._callback_visualizer_objects, rclpy.qos.qos_profile_sensor_data, @@ -146,8 +146,9 @@ def _layer_state_changed(self): def _callback_visualizer_objects(self, msg): # ここでレイヤーを更新する - self._add_visualizer_layer(msg.layer, msg.sub_layer) - self._widget.field_widget.set_visualizer_objects(msg) + for objects in msg.objects: + self._add_visualizer_layer(objects.layer, objects.sub_layer) + self._widget.field_widget.set_visualizer_objects(objects) def _add_visualizer_layer(self, layer: str, sub_layer: str, state=Qt.Unchecked): # レイヤーに重複しないように項目を追加する diff --git a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt index 4ad563c74..c19fc1821 100644 --- a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt +++ b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/Color.msg" "msg/Objects.msg" + "msg/ObjectsArray.msg" "msg/Point.msg" "msg/ShapeAnnotation.msg" "msg/ShapeArc.msg" diff --git a/consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg b/consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg new file mode 100644 index 000000000..f7830ffc5 --- /dev/null +++ b/consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg @@ -0,0 +1 @@ +Objects[] objects diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index 7d94814fd..a4299fcad 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -40,6 +40,8 @@ class LocalPlannerComponent : public rclcpp::Node declare_parameter("planner", "gridmap"); auto planner_str = get_parameter("planner").as_string(); + crane::ConsaiVisualizerBuffer::activate(*this); + process_time_pub = create_publisher("process_time", 10); if (planner_str == "gridmap") { planner = std::make_shared(*this); diff --git a/crane_local_planner/include/crane_local_planner/planner_base.hpp b/crane_local_planner/include/crane_local_planner/planner_base.hpp index 0a4a1c110..c6f7adca5 100644 --- a/crane_local_planner/include/crane_local_planner/planner_base.hpp +++ b/crane_local_planner/include/crane_local_planner/planner_base.hpp @@ -10,6 +10,7 @@ #include #include #include +#include namespace crane { @@ -17,14 +18,14 @@ class LocalPlannerBase { public: LocalPlannerBase(std::string name, rclcpp::Node & node) + : visualizer(std::make_unique("local_planner", name)) { world_model = std::make_shared(node); - visualizer = std::make_shared(node, name); } virtual crane_msgs::msg::RobotCommands calculateRobotCommand( const crane_msgs::msg::RobotCommands & msg) = 0; - ConsaiVisualizerWrapper::SharedPtr visualizer; + ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; WorldModelWrapper::SharedPtr world_model; }; diff --git a/crane_local_planner/include/crane_local_planner/simple_planner.hpp b/crane_local_planner/include/crane_local_planner/simple_planner.hpp index 7a9843b4a..1381549f9 100644 --- a/crane_local_planner/include/crane_local_planner/simple_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/simple_planner.hpp @@ -48,7 +48,6 @@ class SimplePlanner : public LocalPlannerBase command.local_planner_config.max_velocity = MAX_VEL; } } - visualizer->publish(); return commands; } diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index f3a920854..3d4a4c6e3 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -107,7 +107,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo } } - planner->visualizer->publish(); + crane::ConsaiVisualizerBuffer::publish(); } } // namespace crane diff --git a/crane_robot_receiver/src/robot_receiver_node.cpp b/crane_robot_receiver/src/robot_receiver_node.cpp index 2c195680f..f9aa0be93 100644 --- a/crane_robot_receiver/src/robot_receiver_node.cpp +++ b/crane_robot_receiver/src/robot_receiver_node.cpp @@ -264,8 +264,10 @@ class RobotReceiverNode : public rclcpp::Node { public: explicit RobotReceiverNode(uint8_t robot_num = 10) - : rclcpp::Node("robot_receiver_node"), consai_visualizer_wrapper(*this, "robot_feedback") + : rclcpp::Node("robot_receiver_node"), + visualizer(std::make_unique("robot_receiver")) { + crane::ConsaiVisualizerBuffer::activate(*this); publisher = create_publisher("/robot_feedback", 10); for (int i = 0; i < robot_num; i++) { @@ -331,6 +333,8 @@ class RobotReceiverNode : public rclcpp::Node msg.feedback.push_back(robot_feedback_msg); } publisher->publish(msg); + visualizer->flush(); + crane::ConsaiVisualizerBuffer::publish(); }); } @@ -340,7 +344,7 @@ class RobotReceiverNode : public rclcpp::Node rclcpp::Publisher::SharedPtr publisher; - crane::ConsaiVisualizerWrapper consai_visualizer_wrapper; + crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; }; int main(int argc, char * argv[]) 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 f6b646748..a7cae3b91 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 @@ -18,7 +18,7 @@ class BallNearByPositioner : public SkillBase public: explicit BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & base); - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override { os << "[BallNearByPositioner]"; } }; diff --git a/crane_robot_skills/include/crane_robot_skills/freekick_saver.hpp b/crane_robot_skills/include/crane_robot_skills/freekick_saver.hpp index 66c9f0722..284321cce 100644 --- a/crane_robot_skills/include/crane_robot_skills/freekick_saver.hpp +++ b/crane_robot_skills/include/crane_robot_skills/freekick_saver.hpp @@ -26,7 +26,7 @@ class FreeKickSaver : public SkillBase { } - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override + Status update() override { auto cmd = std::make_shared(command); auto & ball = world_model()->ball.pos; diff --git a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp index 0ef7fb6ec..a7b209dd3 100644 --- a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp +++ b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp @@ -18,7 +18,7 @@ class GetBallContact : public SkillBase public: explicit GetBallContact(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & out) const override; diff --git a/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp b/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp index 5a7c5901c..8b067173d 100644 --- a/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp @@ -20,7 +20,7 @@ class GoOverBall : public SkillBase public: explicit GoOverBall(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & out) const override; 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 6d657dc0c..a52125070 100644 --- a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp @@ -19,7 +19,7 @@ class GoalKick : public SkillBase public: explicit GoalKick(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override { os << "[GoalKick] "; } diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp index 0a11e07fc..63a9c814a 100644 --- a/crane_robot_skills/include/crane_robot_skills/goalie.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goalie.hpp @@ -21,11 +21,11 @@ class Goalie : public SkillBase public: explicit Goalie(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; - void emitBallFromPenaltyArea(const ConsaiVisualizerWrapper::SharedPtr & visualizer); + void emitBallFromPenaltyArea(); - void inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & visualizer); + void inplay(bool enable_emit); void print(std::ostream & os) const override { os << "[Goalie] " << phase; } diff --git a/crane_robot_skills/include/crane_robot_skills/idle.hpp b/crane_robot_skills/include/crane_robot_skills/idle.hpp index 5dbc2914c..e00dc8e7a 100644 --- a/crane_robot_skills/include/crane_robot_skills/idle.hpp +++ b/crane_robot_skills/include/crane_robot_skills/idle.hpp @@ -18,7 +18,7 @@ class Idle : public SkillBase public: explicit Idle(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("Idle", base) {} - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override + Status update() override { command.stopHere(); return Status::RUNNING; diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp index 4cdad260c..9af5ff665 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp @@ -23,7 +23,7 @@ class KickoffSupport : public SkillBase setParameter("target_y", 0.5f); } - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override + Status update() override { Point target(getParameter("target_x"), getParameter("target_y")); command.lookAtBallFrom(target).setDribblerTargetPosition(target); diff --git a/crane_robot_skills/include/crane_robot_skills/marker.hpp b/crane_robot_skills/include/crane_robot_skills/marker.hpp index bd1c5901d..fd48d9b5e 100644 --- a/crane_robot_skills/include/crane_robot_skills/marker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/marker.hpp @@ -23,7 +23,7 @@ class Marker : public SkillBase public: explicit Marker(RobotCommandWrapperBase::SharedPtr & base); - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override { os << "[Marker]"; } }; diff --git a/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp b/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp index bc52ff6ca..7bbe10183 100644 --- a/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp @@ -28,7 +28,7 @@ class MoveWithBall : public SkillBase public: explicit MoveWithBall(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; Point getTargetPoint(const Point & target_pos); diff --git a/crane_robot_skills/include/crane_robot_skills/receive.hpp b/crane_robot_skills/include/crane_robot_skills/receive.hpp index d48099bf7..f3e9832f1 100644 --- a/crane_robot_skills/include/crane_robot_skills/receive.hpp +++ b/crane_robot_skills/include/crane_robot_skills/receive.hpp @@ -18,11 +18,11 @@ class Receive : public SkillBase public: explicit Receive(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override { os << "[Receive]"; } - Point getInterceptionPoint(const ConsaiVisualizerWrapper::SharedPtr & visualizer) const; + Point getInterceptionPoint() const; }; } // namespace crane::skills diff --git a/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp b/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp index a468427be..b45f72f4d 100644 --- a/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp +++ b/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp @@ -14,13 +14,13 @@ namespace crane::skills { -#define DEFINE_SKILL_COMMAND(name, type) \ - class Cmd##name : public SkillBase \ - { \ - public: \ - explicit Cmd##name(RobotCommandWrapperBase::SharedPtr & base); \ - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; \ - void print(std::ostream & os) const override; \ +#define DEFINE_SKILL_COMMAND(name, type) \ + class Cmd##name : public SkillBase \ + { \ + public: \ + explicit Cmd##name(RobotCommandWrapperBase::SharedPtr & base); \ + Status update() override; \ + void print(std::ostream & os) const override; \ } DEFINE_SKILL_COMMAND(KickWithChip, Position); diff --git a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp index 5970ccbf7..af45f6f33 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp @@ -24,7 +24,7 @@ class SimpleKickOff : public SkillBase public: explicit SimpleKickOff(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override { os << "[SimpleKickOff]"; } diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 6a4b0eac5..6e95ad518 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -126,6 +126,7 @@ class SkillInterface const std::string & name, uint8_t id, const std::shared_ptr & wm) : name(name), command_base(std::make_shared(name, id, wm)), + visualizer(std::make_unique("skill", name)), target_theta_context(getContextReference("target_theta")), dribble_power_context(getContextReference("dribble_power")), kick_power_context(getContextReference("kick_power")), @@ -137,6 +138,7 @@ class SkillInterface SkillInterface(const std::string & name, RobotCommandWrapperBase::SharedPtr command) : name(name), command_base(command), + visualizer(std::make_unique("skill", name)), target_theta_context(getContextReference("target_theta")), dribble_power_context(getContextReference("dribble_power")), kick_power_context(getContextReference("kick_power")), @@ -149,7 +151,6 @@ class SkillInterface const std::string name; virtual Status run( - const ConsaiVisualizerWrapper::SharedPtr & visualizer, std::optional> parameters_opt = std::nullopt) = 0; @@ -233,6 +234,8 @@ class SkillInterface std::unordered_map contexts; + crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + Status status = Status::RUNNING; void updateDefaultContexts() @@ -271,7 +274,6 @@ class SkillBase : public SkillInterface } Status run( - const ConsaiVisualizerWrapper::SharedPtr & visualizer, std::optional> parameters_opt = std::nullopt) override { @@ -283,12 +285,13 @@ class SkillBase : public SkillInterface command_base->latest_msg.current_pose.y = command_base->robot->pose.pos.y(); command_base->latest_msg.current_pose.theta = command_base->robot->pose.theta; - auto ret = update(visualizer); + auto ret = update(); updateDefaultContexts(); + visualizer->flush(); return ret; } - virtual Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) = 0; + virtual Status update() = 0; crane_msgs::msg::RobotCommand getRobotCommand() override { return command.getMsg(); } @@ -306,7 +309,7 @@ template ; + using StateFunctionType = std::function; SkillBaseWithState( const std::string & name, uint8_t id, const std::shared_ptr & wm, @@ -325,7 +328,6 @@ class SkillBaseWithState : public SkillInterface } Status run( - const ConsaiVisualizerWrapper::SharedPtr & visualizer, std::optional> parameters_opt = std::nullopt) override { @@ -339,8 +341,9 @@ class SkillBaseWithState : public SkillInterface command_base->latest_msg.current_pose.y = command_base->robot->pose.pos.y(); command_base->latest_msg.current_pose.theta = command_base->robot->pose.theta; - auto ret = state_functions[state_machine.getCurrentState()](visualizer); + auto ret = state_functions[state_machine.getCurrentState()](); updateDefaultContexts(); + visualizer->flush(); return ret; } diff --git a/crane_robot_skills/include/crane_robot_skills/sleep.hpp b/crane_robot_skills/include/crane_robot_skills/sleep.hpp index bb428e922..9a4a5e22b 100644 --- a/crane_robot_skills/include/crane_robot_skills/sleep.hpp +++ b/crane_robot_skills/include/crane_robot_skills/sleep.hpp @@ -18,7 +18,7 @@ class Sleep : public SkillBase public: explicit Sleep(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override; diff --git a/crane_robot_skills/include/crane_robot_skills/sub_attacker.hpp b/crane_robot_skills/include/crane_robot_skills/sub_attacker.hpp index 8630e4234..59f1a613d 100644 --- a/crane_robot_skills/include/crane_robot_skills/sub_attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/sub_attacker.hpp @@ -22,7 +22,7 @@ class SubAttacker : public SkillBase public: explicit SubAttacker(RobotCommandWrapperBase::SharedPtr & base); - Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override { os << "[SubAttacker]"; } diff --git a/crane_robot_skills/include/crane_robot_skills/teleop.hpp b/crane_robot_skills/include/crane_robot_skills/teleop.hpp index 2f656cf1c..700f126de 100644 --- a/crane_robot_skills/include/crane_robot_skills/teleop.hpp +++ b/crane_robot_skills/include/crane_robot_skills/teleop.hpp @@ -20,7 +20,7 @@ class Teleop : public SkillBase, public rclcpp::Nod public: explicit Teleop(RobotCommandWrapperBase::SharedPtr & base); - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; + Status update() override; void print(std::ostream & os) const override { os << "[Teleop]"; } diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 04386b7a4..74fc1cff4 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -21,12 +21,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) { receive_skill.setParameter("policy", std::string("closest")); setParameter("receiver_id", -1); - addStateFunction( - AttackerState::ENTRY_POINT, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - std::cout << "ENTRY_POINT" << std::endl; - return Status::RUNNING; - }); + addStateFunction(AttackerState::ENTRY_POINT, [this]() -> Status { + std::cout << "ENTRY_POINT" << std::endl; + return Status::RUNNING; + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::FORCED_PASS, [this]() -> bool { // セットプレイのときは強制パス @@ -47,47 +45,45 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) } }); - addStateFunction( - AttackerState::FORCED_PASS, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - switch (forced_pass_phase) { - case 0: { - // 90度別の方向で構えて敵のプレッシャーをかわす - Point target = world_model()->ball.pos + - getVerticalVec(kick_target - world_model()->ball.pos).normalized() * 0.3; - command.setTargetPosition(target).lookAtBallFrom(target).enableBallAvoidance(); - if (robot()->getDistance(target) < 0.1) { - forced_pass_phase = 1; - } - break; + addStateFunction(AttackerState::FORCED_PASS, [this]() -> Status { + switch (forced_pass_phase) { + case 0: { + // 90度別の方向で構えて敵のプレッシャーをかわす + Point target = world_model()->ball.pos + + getVerticalVec(kick_target - world_model()->ball.pos).normalized() * 0.3; + command.setTargetPosition(target).lookAtBallFrom(target).enableBallAvoidance(); + if (robot()->getDistance(target) < 0.1) { + forced_pass_phase = 1; } - case 1: { - // パス - command.disableBallAvoidance(); - kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("kick_power", 0.8); - int receiver_id = getParameter("receiver_id"); - if (receiver_id != -1) { - kick_target = world_model()->getOurRobot(receiver_id)->pose.pos; - } - Segment kick_line{world_model()->ball.pos, kick_target}; - // 近くに敵ロボットがいればチップキック - if (const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - not enemy_robots.empty()) { - const auto & [nearest_enemy, enemy_distance] = - world_model()->getNearestRobotWithDistanceFromSegment(kick_line, enemy_robots); - if (enemy_distance < 0.4 && nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) { - kick_skill.setParameter("kick_with_chip", true); - } + break; + } + case 1: { + // パス + command.disableBallAvoidance(); + kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("kick_power", 0.8); + int receiver_id = getParameter("receiver_id"); + if (receiver_id != -1) { + kick_target = world_model()->getOurRobot(receiver_id)->pose.pos; + } + Segment kick_line{world_model()->ball.pos, kick_target}; + // 近くに敵ロボットがいればチップキック + if (const auto enemy_robots = world_model()->theirs.getAvailableRobots(); + not enemy_robots.empty()) { + const auto & [nearest_enemy, enemy_distance] = + world_model()->getNearestRobotWithDistanceFromSegment(kick_line, enemy_robots); + if (enemy_distance < 0.4 && nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) { + kick_skill.setParameter("kick_with_chip", true); } - kick_skill.run(visualizer); - break; } - default: - return Status::FAILURE; + kick_skill.run(); + break; } - return Status::RUNNING; - }); + default: + return Status::FAILURE; + } + return Status::RUNNING; + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool { return not world_model()->isOurBallByBallOwnerCalculator() && @@ -99,14 +95,12 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return world_model()->isOurBallByBallOwnerCalculator() or world_model()->ball.isStopped(0.2); }); - addStateFunction( - AttackerState::CUT_THEIR_PASS, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5); - receive_skill.setParameter("enable_redirect", false); - receive_skill.setParameter("policy", std::string("min_slack")); - return receive_skill.run(visualizer); - }); + addStateFunction(AttackerState::CUT_THEIR_PASS, [this]() -> Status { + visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5); + receive_skill.setParameter("enable_redirect", false); + receive_skill.setParameter("policy", std::string("min_slack")); + return receive_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool { // 止まっているボールを相手が持っているとき @@ -122,12 +116,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return world_model()->isOurBallByBallOwnerCalculator(); }); - addStateFunction( - AttackerState::STEAL_BALL, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0); - return steal_ball_skill.run(visualizer); - }); + addStateFunction(AttackerState::STEAL_BALL, [this]() -> Status { + visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0); + return steal_ball_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::REDIRECT_GOAL_KICK, [this]() -> bool { // ボールが遠くにいる @@ -158,32 +150,30 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) } }); - addStateFunction( - AttackerState::REDIRECT_GOAL_KICK, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - auto target = [&]() -> Point { - double angle = GoalKick::getBestAngleToShootFromPoint( - 10.0 * M_PI / 180., robot()->pose.pos, world_model()); - Segment shoot_line{robot()->pose.pos, robot()->pose.pos + getNormVec(angle) * 10.}; - Segment goal_line; - goal_line.first << world_model()->getTheirGoalCenter().x(), - -world_model()->field_size.y() * 0.5; - goal_line.second << world_model()->getTheirGoalCenter().x(), - world_model()->field_size.y() * 0.5; - if (auto intersection_points = getIntersections(shoot_line, goal_line); - intersection_points.empty()) { - return world_model()->getTheirGoalCenter(); - } else { - return intersection_points.front(); - } - }(); + addStateFunction(AttackerState::REDIRECT_GOAL_KICK, [this]() -> Status { + auto target = [&]() -> Point { + double angle = GoalKick::getBestAngleToShootFromPoint( + 10.0 * M_PI / 180., robot()->pose.pos, world_model()); + Segment shoot_line{robot()->pose.pos, robot()->pose.pos + getNormVec(angle) * 10.}; + Segment goal_line; + goal_line.first << world_model()->getTheirGoalCenter().x(), + -world_model()->field_size.y() * 0.5; + goal_line.second << world_model()->getTheirGoalCenter().x(), + world_model()->field_size.y() * 0.5; + if (auto intersection_points = getIntersections(shoot_line, goal_line); + intersection_points.empty()) { + return world_model()->getTheirGoalCenter(); + } else { + return intersection_points.front(); + } + }(); - receive_skill.setParameter("enable_redirect", false); - receive_skill.setParameter("redirect_target", target); - receive_skill.setParameter("policy", std::string("closest")); - receive_skill.setParameter("redirect_kick_power", 0.8); - return receive_skill.run(visualizer); - }); + receive_skill.setParameter("enable_redirect", false); + receive_skill.setParameter("redirect_target", target); + receive_skill.setParameter("policy", std::string("closest")); + receive_skill.setParameter("redirect_kick_power", 0.8); + return receive_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::GOAL_KICK, [this]() -> bool { auto [best_angle, goal_angle_width] = @@ -198,28 +188,24 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return world_model()->ball.isMoving(1.0); }); - addStateFunction( - AttackerState::GOAL_KICK, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - goal_kick_skill.setParameter("dot_threshold", 0.95); - goal_kick_skill.setParameter("キック角度の最低要求精度[deg]", 5.0); - return goal_kick_skill.run(visualizer); - }); + addStateFunction(AttackerState::GOAL_KICK, [this]() -> Status { + goal_kick_skill.setParameter("dot_threshold", 0.95); + goal_kick_skill.setParameter("キック角度の最低要求精度[deg]", 5.0); + return goal_kick_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::CLEARING_KICK, [this]() -> bool { // 未実装：やばいときに蹴る return false; }); - addStateFunction( - AttackerState::CLEARING_KICK, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - kick_skill.setParameter("kick_power", 0.8); - kick_skill.setParameter("dot_threshold", 0.9); - kick_skill.setParameter("kick_with_chip", true); - return kick_skill.run(visualizer); - }); + addStateFunction(AttackerState::CLEARING_KICK, [this]() -> Status { + kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); + kick_skill.setParameter("kick_power", 0.8); + kick_skill.setParameter("dot_threshold", 0.9); + kick_skill.setParameter("kick_with_chip", true); + return kick_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::STANDARD_PASS, [this]() -> bool { if (robot()->getDistance(world_model()->ball.pos) > 1.0 or world_model()->ball.isMoving(1.0)) { @@ -282,33 +268,30 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return world_model()->ball.isMoving(1.0); }); - addStateFunction( - AttackerState::STANDARD_PASS, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - int receiver_id = getParameter("receiver_id"); - if (receiver_id != -1) { - kick_target = world_model()->getOurRobot(receiver_id)->pose.pos; - } + addStateFunction(AttackerState::STANDARD_PASS, [this]() -> Status { + int receiver_id = getParameter("receiver_id"); + if (receiver_id != -1) { + kick_target = world_model()->getOurRobot(receiver_id)->pose.pos; + } - auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); - const auto enemy_robots = world_model()->theirs.getAvailableRobots(); + auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); + const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - visualizer->addLine(world_model()->ball.pos, kick_target, 1, "red"); + visualizer->addLine(world_model()->ball.pos, kick_target, 1, "red"); - kick_skill.setParameter("target", kick_target); - Segment ball_to_target{world_model()->ball.pos, kick_target}; - if (not enemy_robots.empty()) { - auto [nearest_enemy, enemy_distance] = - world_model()->getNearestRobotWithDistanceFromSegment( - ball_to_target, world_model()->theirs.getAvailableRobots()); - if (nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) { - kick_skill.setParameter("kick_with_chip", true); - } + kick_skill.setParameter("target", kick_target); + Segment ball_to_target{world_model()->ball.pos, kick_target}; + if (not enemy_robots.empty()) { + auto [nearest_enemy, enemy_distance] = world_model()->getNearestRobotWithDistanceFromSegment( + ball_to_target, world_model()->theirs.getAvailableRobots()); + if (nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) { + kick_skill.setParameter("kick_with_chip", true); } - kick_skill.setParameter("kick_power", 0.5); - kick_skill.setParameter("dot_threshold", 0.97); - return kick_skill.run(visualizer); - }); + } + kick_skill.setParameter("kick_power", 0.5); + kick_skill.setParameter("dot_threshold", 0.97); + return kick_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> bool { // ボールが近く、相手コートにいるとき（本当にチャンスが無いとき(隙間が1deg以下)は除外） @@ -327,10 +310,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) }); addStateFunction( - AttackerState::LOW_CHANCE_GOAL_KICK, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - return goal_kick_skill.run(visualizer); - }); + AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> Status { return goal_kick_skill.run(); }); addTransition( AttackerState::ENTRY_POINT, AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> bool { @@ -347,16 +327,14 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return not world_model()->isOurBallByBallOwnerCalculator(); }); - addStateFunction( - AttackerState::MOVE_BALL_TO_OPPONENT_HALF, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - kick_skill.setParameter("kick_power", 0.8); - kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("kick_with_chip", true); - command.disableBallAvoidance(); - return kick_skill.run(visualizer); - }); + addStateFunction(AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> Status { + kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); + kick_skill.setParameter("kick_power", 0.8); + kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("kick_with_chip", true); + command.disableBallAvoidance(); + return kick_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::RECEIVE_BALL, [this]() -> bool { if (world_model()->ball.vel.norm() < 0.5) { @@ -382,14 +360,12 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) } }); - addStateFunction( - AttackerState::RECEIVE_BALL, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - receive_skill.setParameter("enable_redirect", true); - receive_skill.setParameter("dribble_power", 0.0); - receive_skill.setParameter("enable_software_bumper", false); - return receive_skill.run(visualizer); - }); + addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status { + receive_skill.setParameter("enable_redirect", true); + receive_skill.setParameter("dribble_power", 0.0); + receive_skill.setParameter("enable_software_bumper", false); + return receive_skill.run(); + }); addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { // 一定以上ボールに触れたら終了 @@ -402,15 +378,13 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return true; }); - addStateFunction( - AttackerState::KICK_TO_GOAL, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - kick_skill.setParameter("kick_power", 0.8); - // kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("kick_with_chip", false); - return kick_skill.run(visualizer); - }); + addStateFunction(AttackerState::KICK_TO_GOAL, [this]() -> Status { + kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); + kick_skill.setParameter("kick_power", 0.8); + // kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("kick_with_chip", false); + return kick_skill.run(); + }); addTransition(AttackerState::ENTRY_POINT, AttackerState::KICK_TO_GOAL, [this]() -> bool { // どこにも当てはまらないときはゴールに向かってシュート diff --git a/crane_robot_skills/src/ball_nearby_positioner.cpp b/crane_robot_skills/src/ball_nearby_positioner.cpp index 912d07811..e17a8f286 100644 --- a/crane_robot_skills/src/ball_nearby_positioner.cpp +++ b/crane_robot_skills/src/ball_nearby_positioner.cpp @@ -23,7 +23,7 @@ BallNearByPositioner::BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & setParameter("margin_distance", 0.6); } -Status BallNearByPositioner::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status BallNearByPositioner::update() { auto situation = world_model()->play_situation.getSituationCommandID(); double distance_from_ball = [&]() { diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp index 665031e9a..5e554909b 100644 --- a/crane_robot_skills/src/get_ball_contact.cpp +++ b/crane_robot_skills/src/get_ball_contact.cpp @@ -16,8 +16,7 @@ GetBallContact::GetBallContact(RobotCommandWrapperBase::SharedPtr & base) setParameter("dribble_power", 0.5); } -Status GetBallContact::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status GetBallContact::update() { if ( robot()->ball_contact.getContactDuration() > diff --git a/crane_robot_skills/src/go_over_ball.cpp b/crane_robot_skills/src/go_over_ball.cpp index bcf5bc8dd..51f182752 100644 --- a/crane_robot_skills/src/go_over_ball.cpp +++ b/crane_robot_skills/src/go_over_ball.cpp @@ -24,7 +24,7 @@ GoOverBall::GoOverBall(RobotCommandWrapperBase::SharedPtr & base) setParameter("reach_threshold", 0.05); } -Status GoOverBall::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status GoOverBall::update() { if (not has_started) { Point next_target{getParameter("next_target_x"), getParameter("next_target_y")}; diff --git a/crane_robot_skills/src/goal_kick.cpp b/crane_robot_skills/src/goal_kick.cpp index c03c0be9d..d57680b3b 100644 --- a/crane_robot_skills/src/goal_kick.cpp +++ b/crane_robot_skills/src/goal_kick.cpp @@ -20,7 +20,7 @@ GoalKick::GoalKick(RobotCommandWrapperBase::SharedPtr & base) kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); } -Status GoalKick::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status GoalKick::update() { double best_angle = getBestAngleToShootFromPoint( getParameter("キック角度の最低要求精度[deg]") * M_PI / 180., world_model()->ball.pos, @@ -30,7 +30,7 @@ Status GoalKick::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) 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); + return kick_skill.run(); } double GoalKick::getBestAngleToShootFromPoint( diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index a848e6e2a..8c09f0581 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -17,7 +17,7 @@ Goalie::Goalie(RobotCommandWrapperBase::SharedPtr & base) setParameter("block_distance", 1.0); } -Status Goalie::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status Goalie::update() { auto situation = world_model()->play_situation.getSituationCommandID(); if (getParameter("run_inplay")) { @@ -33,10 +33,10 @@ Status Goalie::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) [[fallthrough]]; case crane_msgs::msg::PlaySituation::THEIR_PENALTY_START: phase = "ペナルティキック"; - inplay(false, visualizer); + inplay(false); break; default: - inplay(true, visualizer); + inplay(true); break; } @@ -44,7 +44,7 @@ Status Goalie::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) return Status::RUNNING; } -void Goalie::emitBallFromPenaltyArea(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +void Goalie::emitBallFromPenaltyArea() { Point ball = world_model()->ball.pos; // パスできるロボットのリストアップ @@ -82,12 +82,12 @@ void Goalie::emitBallFromPenaltyArea(const ConsaiVisualizerWrapper::SharedPtr & kick_skill.setParameter("target", pass_target); kick_skill.setParameter("kick_power", 1.0); kick_skill.setParameter("chip_kick", true); - kick_skill.run(visualizer); + kick_skill.run(); // 追加のコマンド command.disableGoalAreaAvoidance().disableRuleAreaAvoidance(); } -void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & visualizer) +void Goalie::inplay(bool enable_emit) { auto goals = world_model()->getOurGoalPosts(); const auto & ball = world_model()->ball; @@ -124,7 +124,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & world_model()->point_checker.isFriendPenaltyArea(ball.pos) && enable_emit) { // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す phase = "ボール排出"; - emitBallFromPenaltyArea(visualizer); + emitBallFromPenaltyArea(); } else { phase = ""; const double BLOCK_DIST = getParameter("block_distance"); diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index 2e675baee..f57eef490 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -34,11 +34,10 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) 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; - }); + addStateFunction(KickState::ENTRY_POINT, [this]() { + 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")); @@ -46,26 +45,25 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) 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.3, 1., 2.0); - if (min_slack_pos) { - state << "min_slack: " << min_slack_pos.value().x() << ", " << min_slack_pos.value().y(); - command.setTargetPosition(min_slack_pos.value()).lookAtBallFrom(min_slack_pos.value()); - } else { - // ball_lineとフィールドラインの交点を目指す - Point ball_exit_point = getBallExitPointFromField(0.3); - command.setTargetPosition(ball_exit_point) - .lookAtFrom(world_model()->ball.pos, ball_exit_point); - state << "ball_exit: " << ball_exit_point.x() << ", " << ball_exit_point.y(); - } - visualizer->addPoint(robot()->pose.pos, 0, "", 1., state.str()); - return Status::RUNNING; - }); + addStateFunction(KickState::CHASE_BALL, [this]() { + std::stringstream state; + state << "Kick::CHASE_BALL::"; + // メモ：ボールが近い時はボールから少しずらした位置を目指したほうがいいかも + auto [min_slack_pos, max_slack_pos] = + world_model()->getMinMaxSlackInterceptPoint({robot()}, 5.0, 0.1, -0.3, 1., 2.0); + if (min_slack_pos) { + state << "min_slack: " << min_slack_pos.value().x() << ", " << min_slack_pos.value().y(); + command.setTargetPosition(min_slack_pos.value()).lookAtBallFrom(min_slack_pos.value()); + } else { + // ball_lineとフィールドラインの交点を目指す + Point ball_exit_point = getBallExitPointFromField(0.3); + command.setTargetPosition(ball_exit_point) + .lookAtFrom(world_model()->ball.pos, ball_exit_point); + state << "ball_exit: " << ball_exit_point.x() << ", " << ball_exit_point.y(); + } + visualizer->addPoint(robot()->pose.pos, 0, "", 1., state.str()); + return Status::RUNNING; + }); addTransition(KickState::CHASE_BALL, KickState::AROUND_BALL, [this]() { // ボールが止まったら回り込みへ @@ -87,55 +85,52 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) 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; - }); + addStateFunction(KickState::POSITIVE_REDIRECT_KICK, [this]() { + 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")); - } - command.disableBallAvoidance(); - return receive_skill->update(visualizer); - }); + addStateFunction(KickState::REDIRECT_KICK, [this]() { + 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")); + } + command.disableBallAvoidance(); + return receive_skill->update(); + }); addTransition(KickState::REDIRECT_KICK, KickState::AROUND_BALL, [this]() { // ボールが止まったら回り込みへ @@ -148,42 +143,40 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) 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; + addStateFunction(KickState::AROUND_BALL, [this]() { + 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"); + // 経由ポイント + Point intermediate_point = + ball_pos + (ball_pos - target).normalized() * getParameter("around_interval"); + command.setTargetPosition(intermediate_point) + .lookAtFrom(target, ball_pos) + .enableCollisionAvoidance(); + + // ボールを避けて回り込む + 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).lookAtFrom(target, ball_pos); + } else { + // 経由ポイントへGO command.setTargetPosition(intermediate_point) .lookAtFrom(target, ball_pos) .enableCollisionAvoidance(); - - // ボールを避けて回り込む - 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).lookAtFrom(target, ball_pos); - } else { - // 経由ポイントへGO - command.setTargetPosition(intermediate_point) - .lookAtFrom(target, ball_pos) - .enableCollisionAvoidance(); - } - return Status::RUNNING; - }); + } + return Status::RUNNING; + }); addTransition(KickState::AROUND_BALL, KickState::KICK, [this]() { // 中間地点に到達したらキックへ @@ -194,7 +187,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) return robot()->getDistance(intermediate_point) < 0.05 && robot()->vel.linear.norm() < 0.15; }); - addStateFunction(KickState::KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { + addStateFunction(KickState::KICK, [this]() { visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::KICK"); auto target = getParameter("target"); Point ball_pos = world_model()->ball.pos; diff --git a/crane_robot_skills/src/kickoff_attack.cpp b/crane_robot_skills/src/kickoff_attack.cpp index a39d48e87..ab8fab373 100644 --- a/crane_robot_skills/src/kickoff_attack.cpp +++ b/crane_robot_skills/src/kickoff_attack.cpp @@ -14,39 +14,35 @@ KickoffAttack::KickoffAttack(RobotCommandWrapperBase::SharedPtr & base) setParameter("target_x", 0.0f); setParameter("target_y", 1.0f); setParameter("kick_power", 0.25); - addStateFunction( - KickoffAttackState::PREPARE_KICKOFF, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - if (not go_over_ball) { - go_over_ball = std::make_shared(command_base); - go_over_ball->setParameter("next_target_x", getParameter("target_x")); - go_over_ball->setParameter("next_target_y", getParameter("target_y")); - go_over_ball->setParameter("margin", 0.3); - command.setMaxVelocity(0.5); - command.disableRuleAreaAvoidance(); - } - go_over_ball_status = go_over_ball->run(visualizer); - return Status::RUNNING; - }); + addStateFunction(KickoffAttackState::PREPARE_KICKOFF, [this]() -> Status { + if (not go_over_ball) { + go_over_ball = std::make_shared(command_base); + go_over_ball->setParameter("next_target_x", getParameter("target_x")); + go_over_ball->setParameter("next_target_y", getParameter("target_y")); + go_over_ball->setParameter("margin", 0.3); + command.setMaxVelocity(0.5); + command.disableRuleAreaAvoidance(); + } + go_over_ball_status = go_over_ball->run(); + return Status::RUNNING; + }); addTransition(KickoffAttackState::PREPARE_KICKOFF, KickoffAttackState::KICKOFF, [this]() -> bool { return go_over_ball_status == Status::SUCCESS; }); - addStateFunction( - KickoffAttackState::KICKOFF, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - command.setMaxVelocity(0.5); - command.liftUpDribbler(); - command.kickStraight(getParameter("kick_power")); - command.setTargetPosition(world_model()->ball.pos); - command.setTerminalVelocity(0.5); - command.disableBallAvoidance(); - command.disableRuleAreaAvoidance(); - if (world_model()->ball.vel.norm() > 0.3) { - return Status::SUCCESS; - } else { - return Status::RUNNING; - } - }); + addStateFunction(KickoffAttackState::KICKOFF, [this]() -> Status { + command.setMaxVelocity(0.5); + command.liftUpDribbler(); + command.kickStraight(getParameter("kick_power")); + command.setTargetPosition(world_model()->ball.pos); + command.setTerminalVelocity(0.5); + command.disableBallAvoidance(); + command.disableRuleAreaAvoidance(); + if (world_model()->ball.vel.norm() > 0.3) { + return Status::SUCCESS; + } else { + return Status::RUNNING; + } + }); } } // namespace crane::skills diff --git a/crane_robot_skills/src/marker.cpp b/crane_robot_skills/src/marker.cpp index 83158c9fe..94f5b1ccd 100644 --- a/crane_robot_skills/src/marker.cpp +++ b/crane_robot_skills/src/marker.cpp @@ -16,7 +16,7 @@ Marker::Marker(RobotCommandWrapperBase::SharedPtr & base) setParameter("mark_mode", std::string("save_goal")); } -Status Marker::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status Marker::update() { auto marked_robot = world_model()->getTheirRobot(getParameter("marking_robot_id")); auto enemy_pos = marked_robot->pose.pos; diff --git a/crane_robot_skills/src/move_with_ball.cpp b/crane_robot_skills/src/move_with_ball.cpp index cabb506ae..9b10125ca 100644 --- a/crane_robot_skills/src/move_with_ball.cpp +++ b/crane_robot_skills/src/move_with_ball.cpp @@ -27,7 +27,7 @@ MoveWithBall::MoveWithBall(RobotCommandWrapperBase::SharedPtr & base) setParameter("ball_stabilizing_time", 0.5); } -Status MoveWithBall::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status MoveWithBall::update() { command.setMaxVelocity(0.5); Point target_pos = parseTargetPoint(); diff --git a/crane_robot_skills/src/penalty_kick.cpp b/crane_robot_skills/src/penalty_kick.cpp index ac9124d66..8e252e695 100644 --- a/crane_robot_skills/src/penalty_kick.cpp +++ b/crane_robot_skills/src/penalty_kick.cpp @@ -19,17 +19,15 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base) setParameter("start_from_kick", false); setParameter("prepare_margin", 0.6); kick_skill.setParameter("dot_threshold", 0.97); - addStateFunction( - PenaltyKickState::PREPARE, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - Point target = world_model()->ball.pos; - auto margin = getParameter("prepare_margin"); - target.x() += world_model()->getOurGoalCenter().x() > 0 ? margin : -margin; - command.setTargetPosition(target); - command.lookAtBall(); - command.disableRuleAreaAvoidance(); - return Status::RUNNING; - }); + addStateFunction(PenaltyKickState::PREPARE, [this]() -> Status { + Point target = world_model()->ball.pos; + auto margin = getParameter("prepare_margin"); + target.x() += world_model()->getOurGoalCenter().x() > 0 ? margin : -margin; + command.setTargetPosition(target); + command.lookAtBall(); + command.disableRuleAreaAvoidance(); + return Status::RUNNING; + }); addTransition(PenaltyKickState::PREPARE, PenaltyKickState::KICK, [this]() { if (getParameter("start_from_kick")) { @@ -39,42 +37,38 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base) crane_msgs::msg::PlaySituation::OUR_PENALTY_START; } }); - addStateFunction( - PenaltyKickState::KICK, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - if (not start_ball_point) { - start_ball_point = world_model()->ball.pos; - } + addStateFunction(PenaltyKickState::KICK, [this]() -> Status { + if (not start_ball_point) { + start_ball_point = world_model()->ball.pos; + } - double minimum_angle_accuracy = 2.0 * M_PI / 180.; - double best_angle = GoalKick::getBestAngleToShootFromPoint( - minimum_angle_accuracy, world_model()->ball.pos, world_model()); - Point best_target = world_model()->ball.pos + getNormVec(best_angle) * 0.5; - visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target"); + double minimum_angle_accuracy = 2.0 * M_PI / 180.; + double best_angle = GoalKick::getBestAngleToShootFromPoint( + minimum_angle_accuracy, world_model()->ball.pos, world_model()); + Point best_target = world_model()->ball.pos + getNormVec(best_angle) * 0.5; + visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target"); - kick_skill.setParameter("target", best_target); + kick_skill.setParameter("target", best_target); - double dist_ball_goal = - std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); - if (dist_ball_goal < world_model()->getDefenseHeight() + 2.0) { - kick_skill.setParameter("kick_power", 0.8); - } else { - kick_skill.setParameter("kick_power", 0.4); - } - kick_skill.run(visualizer); - command.disableRuleAreaAvoidance(); - return Status::RUNNING; - }); + double dist_ball_goal = + std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); + if (dist_ball_goal < world_model()->getDefenseHeight() + 2.0) { + kick_skill.setParameter("kick_power", 0.8); + } else { + kick_skill.setParameter("kick_power", 0.4); + } + kick_skill.run(); + command.disableRuleAreaAvoidance(); + return Status::RUNNING; + }); addTransition(PenaltyKickState::KICK, PenaltyKickState::DONE, [this]() { return world_model()->point_checker.isPenaltyArea(world_model()->ball.pos); }); - addStateFunction( - PenaltyKickState::DONE, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - command.stopHere(); - return Status::RUNNING; - }); + addStateFunction(PenaltyKickState::DONE, [this]() -> Status { + command.stopHere(); + return Status::RUNNING; + }); } } // namespace crane::skills diff --git a/crane_robot_skills/src/receive.cpp b/crane_robot_skills/src/receive.cpp index b8eafe317..a704ceaba 100644 --- a/crane_robot_skills/src/receive.cpp +++ b/crane_robot_skills/src/receive.cpp @@ -21,7 +21,7 @@ Receive::Receive(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("Receive setParameter("redirect_kick_power", 0.3); } -Status Receive::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status Receive::update() { auto offset = [&]() -> Point { Point offset(0, 0); @@ -46,7 +46,7 @@ Status Receive::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) } return offset; }(); - Point interception_point = getInterceptionPoint(visualizer) + offset; + Point interception_point = getInterceptionPoint() + offset; visualizer->addLine(interception_point, robot()->pose.pos, 1, "red", 1., "intercept"); @@ -69,7 +69,7 @@ Status Receive::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) return Status::RUNNING; } -Point Receive::getInterceptionPoint(const ConsaiVisualizerWrapper::SharedPtr & visualizer) const +Point Receive::getInterceptionPoint() const { std::string policy = getParameter("policy"); if (policy.ends_with("slack")) { diff --git a/crane_robot_skills/src/robot_command_as_skill.cpp b/crane_robot_skills/src/robot_command_as_skill.cpp index 52d3fd552..e3cd81f04 100644 --- a/crane_robot_skills/src/robot_command_as_skill.cpp +++ b/crane_robot_skills/src/robot_command_as_skill.cpp @@ -10,14 +10,14 @@ namespace crane::skills { -#define ONE_FRAME_IMPLEMENTATION(name, method) \ - Cmd##name::Cmd##name(RobotCommandWrapperBase::SharedPtr & base) \ - : SkillBase("Cmd" #name, base) {} \ - Status Cmd##name::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) \ - { \ - command.method; \ - return Status::SUCCESS; \ - } \ +#define ONE_FRAME_IMPLEMENTATION(name, method) \ + Cmd##name::Cmd##name(RobotCommandWrapperBase::SharedPtr & base) \ + : SkillBase("Cmd" #name, base) {} \ + Status Cmd##name::update() \ + { \ + command.method; \ + return Status::SUCCESS; \ + } \ void Cmd##name::print([[maybe_unused]] std::ostream & os) const {} CmdKickWithChip::CmdKickWithChip(RobotCommandWrapperBase::SharedPtr & base) @@ -26,8 +26,7 @@ CmdKickWithChip::CmdKickWithChip(RobotCommandWrapperBase::SharedPtr & base) setParameter("power", 0.5); } -Status CmdKickWithChip::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdKickWithChip::update() { command.kickWithChip(getParameter("power")); return Status::SUCCESS; @@ -44,8 +43,7 @@ CmdKickStraight::CmdKickStraight(RobotCommandWrapperBase::SharedPtr & base) setParameter("power", 0.5); } -Status CmdKickStraight::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdKickStraight::update() { command.kickStraight(getParameter("power")); return Status::SUCCESS; @@ -61,7 +59,7 @@ CmdDribble::CmdDribble(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("C setParameter("power", 0.5); } -Status CmdDribble::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdDribble::update() { command.dribble(getParameter("power")); return Status::SUCCESS; @@ -79,8 +77,7 @@ CmdSetVelocity::CmdSetVelocity(RobotCommandWrapperBase::SharedPtr & base) setParameter("y", 0.0); } -Status CmdSetVelocity::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdSetVelocity::update() { command.setVelocity(getParameter("x"), getParameter("y")); return Status::SUCCESS; @@ -101,8 +98,7 @@ CmdSetTargetPosition::CmdSetTargetPosition(RobotCommandWrapperBase::SharedPtr & setParameter("exit_immediately", false); } -Status CmdSetTargetPosition::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdSetTargetPosition::update() { Point target{getParameter("x"), getParameter("y")}; command.setTargetPosition(target, getParameter("tolerance")); @@ -136,8 +132,7 @@ CmdSetDribblerTargetPosition::CmdSetDribblerTargetPosition( setParameter("exit_immediately", false); } -Status CmdSetDribblerTargetPosition::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdSetDribblerTargetPosition::update() { Point target{getParameter("x"), getParameter("y")}; command.setTargetTheta(getParameter("theta"), getParameter("angle_tolerance")); @@ -168,8 +163,7 @@ CmdSetTargetTheta::CmdSetTargetTheta(RobotCommandWrapperBase::SharedPtr & base) setParameter("omega_limit", 10.0); } -Status CmdSetTargetTheta::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdSetTargetTheta::update() { command.setTargetTheta(getParameter("theta"), getParameter("tolerance")) .setOmegaLimit(getParameter("omega_limit")); @@ -185,7 +179,7 @@ CmdStopHere::CmdStopHere(RobotCommandWrapperBase::SharedPtr & base) : SkillBase( { } -Status CmdStopHere::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdStopHere::update() { command.stopHere(); return Status::SUCCESS; @@ -211,8 +205,7 @@ CmdSetMaxVelocity::CmdSetMaxVelocity(RobotCommandWrapperBase::SharedPtr & base) setParameter("max_velocity", 0.5); } -Status CmdSetMaxVelocity::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdSetMaxVelocity::update() { command.setMaxVelocity(getParameter("max_velocity")); return Status::SUCCESS; @@ -229,8 +222,7 @@ CmdSetMaxAcceleration::CmdSetMaxAcceleration(RobotCommandWrapperBase::SharedPtr setParameter("max_acceleration", 0.5); } -Status CmdSetMaxAcceleration::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdSetMaxAcceleration::update() { command.setMaxAcceleration(getParameter("max_acceleration")); return Status::SUCCESS; @@ -247,8 +239,7 @@ CmdSetTerminalVelocity::CmdSetTerminalVelocity(RobotCommandWrapperBase::SharedPt setParameter("terminal_velocity", 0.5); } -Status CmdSetTerminalVelocity::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdSetTerminalVelocity::update() { command.setTerminalVelocity(getParameter("terminal_velocity")); return Status::SUCCESS; @@ -264,8 +255,7 @@ CmdEnableStopFlag::CmdEnableStopFlag(RobotCommandWrapperBase::SharedPtr & base) { } -Status CmdEnableStopFlag::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdEnableStopFlag::update() { command.stopEmergency(true); return Status::SUCCESS; @@ -278,8 +268,7 @@ CmdDisableStopFlag::CmdDisableStopFlag(RobotCommandWrapperBase::SharedPtr & base { } -Status CmdDisableStopFlag::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdDisableStopFlag::update() { command.stopEmergency(false); return Status::SUCCESS; @@ -293,8 +282,7 @@ CmdLiftUpDribbler::CmdLiftUpDribbler(RobotCommandWrapperBase::SharedPtr & base) setParameter("enable", true); } -Status CmdLiftUpDribbler::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdLiftUpDribbler::update() { command.liftUpDribbler(getParameter("enable")); return Status::SUCCESS; @@ -313,7 +301,7 @@ CmdLookAt::CmdLookAt(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("Cmd setParameter("omega_limit", 10.0); } -Status CmdLookAt::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdLookAt::update() { Point target{getParameter("x"), getParameter("y")}; command.lookAt(target, getParameter("theta_tolerance")) @@ -333,7 +321,7 @@ CmdLookAtBall::CmdLookAtBall(RobotCommandWrapperBase::SharedPtr & base) setParameter("omega_limit", 10.0); } -Status CmdLookAtBall::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdLookAtBall::update() { command.lookAtBall(getParameter("theta_tolerance")) .setOmegaLimit(getParameter("omega_limit")); @@ -351,8 +339,7 @@ CmdLookAtBallFrom::CmdLookAtBallFrom(RobotCommandWrapperBase::SharedPtr & base) setParameter("omega_limit", 10.0); } -Status CmdLookAtBallFrom::update( - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status CmdLookAtBallFrom::update() { Point target{getParameter("x"), getParameter("y")}; command.lookAtBallFrom(target, getParameter("theta_tolerance")) diff --git a/crane_robot_skills/src/simple_kickoff.cpp b/crane_robot_skills/src/simple_kickoff.cpp index 56c6dff86..b562ca491 100644 --- a/crane_robot_skills/src/simple_kickoff.cpp +++ b/crane_robot_skills/src/simple_kickoff.cpp @@ -15,9 +15,9 @@ SimpleKickOff::SimpleKickOff(RobotCommandWrapperBase::SharedPtr & base) kick_skill.setParameter("chip_kick", true); } -Status SimpleKickOff::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status SimpleKickOff::update() { kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - return kick_skill.run(visualizer); + return kick_skill.run(); } } // namespace crane::skills diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 9200d5d23..a8197a59b 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -19,14 +19,12 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // マイナスするとコート内も判定される setParameter("コート端判定のオフセット", 0.0); - addStateFunction( - SingleBallPlacementStates::ENTRY_POINT, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - command.stopHere(); - command.setOmegaLimit(10.0); - return Status::RUNNING; - }); + addStateFunction(SingleBallPlacementStates::ENTRY_POINT, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + command.stopHere(); + command.setOmegaLimit(10.0); + return Status::RUNNING; + }); addTransition( SingleBallPlacementStates::ENTRY_POINT, SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, @@ -42,40 +40,38 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); // 端にある場合、コート側からアプローチする - addStateFunction( - SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - if (not pull_back_target) { - pull_back_target = world_model()->ball.pos; - const auto offset = getParameter("コート端判定のオフセット"); - const auto threshold_x = world_model()->field_size.x() * 0.5 + offset; - const auto threshold_y = world_model()->field_size.y() * 0.5 + offset; - if (std::abs(pull_back_target->x()) > threshold_x) { - pull_back_target->x() = std::copysign(threshold_x, pull_back_target->x()); - } - if (std::abs(pull_back_target->y()) > threshold_y) { - pull_back_target->y() = std::copysign(threshold_y, pull_back_target->y()); - } + addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + if (not pull_back_target) { + pull_back_target = world_model()->ball.pos; + const auto offset = getParameter("コート端判定のオフセット"); + const auto threshold_x = world_model()->field_size.x() * 0.5 + offset; + const auto threshold_y = world_model()->field_size.y() * 0.5 + offset; + if (std::abs(pull_back_target->x()) > threshold_x) { + pull_back_target->x() = std::copysign(threshold_x, pull_back_target->x()); + } + if (std::abs(pull_back_target->y()) > threshold_y) { + pull_back_target->y() = std::copysign(threshold_y, pull_back_target->y()); + } - if (pull_back_target->x() > 0.) { - pull_back_target->x() -= 0.3; - } else { - pull_back_target->x() += 0.3; - } - if (pull_back_target->y() > 0.) { - pull_back_target->y() -= 0.3; - } else { - pull_back_target->y() += 0.3; - } + if (pull_back_target->x() > 0.) { + pull_back_target->x() -= 0.3; + } else { + pull_back_target->x() += 0.3; } - command.setTargetPosition(pull_back_target.value()); - command.lookAtBallFrom(pull_back_target.value()).disablePlacementAvoidance(); - command.disableGoalAreaAvoidance().disableBallAvoidance().disableRuleAreaAvoidance(); - double max_vel = std::min(1.5, robot()->getDistance(pull_back_target.value()) + 0.1); - command.setMaxVelocity(max_vel); - return Status::RUNNING; - }); + if (pull_back_target->y() > 0.) { + pull_back_target->y() -= 0.3; + } else { + pull_back_target->y() += 0.3; + } + } + command.setTargetPosition(pull_back_target.value()); + command.lookAtBallFrom(pull_back_target.value()).disablePlacementAvoidance(); + command.disableGoalAreaAvoidance().disableBallAvoidance().disableRuleAreaAvoidance(); + double max_vel = std::min(1.5, robot()->getDistance(pull_back_target.value()) + 0.1); + command.setMaxVelocity(max_vel); + return Status::RUNNING; + }); // 必要ない場合はコート端処理をスキップ addTransition( @@ -98,31 +94,28 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); // PULL_BACK_FROM_EDGE_TOUCH - addStateFunction( - SingleBallPlacementStates::PULL_BACK_FROM_EDGE_TOUCH, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - command.disablePlacementAvoidance() - .disableBallAvoidance() - .disableGoalAreaAvoidance() - .disableRuleAreaAvoidance(); - command.setTargetPosition(world_model()->ball.pos); - command.setMaxVelocity(0.5); - - const auto & ball_pos = world_model()->ball.pos; - const Vector2 field = world_model()->field_size * 0.5; - if ( - std::abs(ball_pos.x()) > (field.x() - 0.05) && - std::abs(ball_pos.y()) > (field.y() - 0.05)) { - // ボールが角にある場合は引っ張る - command.dribble(0.5); - } else { - // 角ではない場合は蹴る - command.dribble(0.2); - command.kickStraight(0.15); - } - return skill_status; - }); + addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_TOUCH, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + command.disablePlacementAvoidance() + .disableBallAvoidance() + .disableGoalAreaAvoidance() + .disableRuleAreaAvoidance(); + command.setTargetPosition(world_model()->ball.pos); + command.setMaxVelocity(0.5); + + const auto & ball_pos = world_model()->ball.pos; + const Vector2 field = world_model()->field_size * 0.5; + if ( + std::abs(ball_pos.x()) > (field.x() - 0.05) && std::abs(ball_pos.y()) > (field.y() - 0.05)) { + // ボールが角にある場合は引っ張る + command.dribble(0.5); + } else { + // 角ではない場合は蹴る + command.dribble(0.2); + command.kickStraight(0.15); + } + return skill_status; + }); // skill_status == Status::SUCCESSの場合に次のステートへ addTransition( @@ -157,20 +150,18 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); // PULL_BACK_FROM_EDGE_PULL - addStateFunction( - SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PULL, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - command.setDribblerTargetPosition(pull_back_target.value()); - // 角度はそのまま引っ張りたいので指定はしない - command.dribble(0.6); - command.setMaxVelocity(0.15); - command.disablePlacementAvoidance(); - command.disableGoalAreaAvoidance(); - command.disableBallAvoidance(); - command.disableRuleAreaAvoidance(); - return Status::RUNNING; - }); + addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PULL, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + command.setDribblerTargetPosition(pull_back_target.value()); + // 角度はそのまま引っ張りたいので指定はしない + command.dribble(0.6); + command.setMaxVelocity(0.15); + command.disablePlacementAvoidance(); + command.disableGoalAreaAvoidance(); + command.disableBallAvoidance(); + command.disableRuleAreaAvoidance(); + return Status::RUNNING; + }); // pull_back_targetに到着したら始めに戻る（GO_OVER_BALLに転送される） addTransition( @@ -184,98 +175,92 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() { return robot()->getDistance(world_model()->ball.pos) > 0.15; }); - addStateFunction( - SingleBallPlacementStates::GO_OVER_BALL, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - command.setMaxVelocity(1.5); - Point placement_target; - placement_target << getParameter("placement_x"), getParameter("placement_y"); - const auto & ball_pos = world_model()->ball.pos; - Point target = ball_pos + (ball_pos - placement_target).normalized() * 0.3; - // ボールを避けて回り込む - if ( - ((robot()->pose.pos - ball_pos).normalized()) - .dot((placement_target - ball_pos).normalized()) > 0.1) { - Point around_point = [&]() { - Vector2 vertical_vec = getVerticalVec((target - ball_pos).normalized()) * 0.3; - 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); - } else { - command.setTargetPosition(target); - } - if (robot()->getDistance(world_model()->ball.pos) < 0.2) { - // ロボットがボールに近い場合は一度引きの動作を入れる - // これは端からのPULLが終わった後の誤作動を防ぐための動きである - target << 0, 0; - } - command.lookAtBallFrom(target); - command.disablePlacementAvoidance(); - command.disableGoalAreaAvoidance(); - command.enableBallAvoidance(); - command.disableRuleAreaAvoidance(); - command.dribble(0.0); - - if (robot()->getDistance(target) < 0.05) { - skill_status = Status::SUCCESS; - } else { - skill_status = Status::RUNNING; - } - return Status::RUNNING; - }); + addStateFunction(SingleBallPlacementStates::GO_OVER_BALL, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + command.setMaxVelocity(1.5); + Point placement_target; + placement_target << getParameter("placement_x"), getParameter("placement_y"); + const auto & ball_pos = world_model()->ball.pos; + Point target = ball_pos + (ball_pos - placement_target).normalized() * 0.3; + // ボールを避けて回り込む + if ( + ((robot()->pose.pos - ball_pos).normalized()) + .dot((placement_target - ball_pos).normalized()) > 0.1) { + Point around_point = [&]() { + Vector2 vertical_vec = getVerticalVec((target - ball_pos).normalized()) * 0.3; + 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); + } else { + command.setTargetPosition(target); + } + if (robot()->getDistance(world_model()->ball.pos) < 0.2) { + // ロボットがボールに近い場合は一度引きの動作を入れる + // これは端からのPULLが終わった後の誤作動を防ぐための動きである + target << 0, 0; + } + command.lookAtBallFrom(target); + command.disablePlacementAvoidance(); + command.disableGoalAreaAvoidance(); + command.enableBallAvoidance(); + command.disableRuleAreaAvoidance(); + command.dribble(0.0); + + if (robot()->getDistance(target) < 0.05) { + skill_status = Status::SUCCESS; + } else { + skill_status = Status::RUNNING; + } + return Status::RUNNING; + }); addTransition( SingleBallPlacementStates::GO_OVER_BALL, SingleBallPlacementStates::CONTACT_BALL, [this]() { return skill_status == Status::SUCCESS; }); - addStateFunction( - SingleBallPlacementStates::CONTACT_BALL, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - if (not get_ball_contact) { - get_ball_contact = std::make_shared(command_base); - } + addStateFunction(SingleBallPlacementStates::CONTACT_BALL, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + if (not get_ball_contact) { + get_ball_contact = std::make_shared(command_base); + } - skill_status = get_ball_contact->run(visualizer); - command.disablePlacementAvoidance(); - command.setMaxVelocity(0.5); - command.setMaxAcceleration(1.0); + skill_status = get_ball_contact->run(); + command.disablePlacementAvoidance(); + command.setMaxVelocity(0.5); + command.setMaxAcceleration(1.0); - return Status::RUNNING; - }); + return Status::RUNNING; + }); addTransition( SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { return skill_status == Status::SUCCESS; }); - addStateFunction( - SingleBallPlacementStates::MOVE_TO_TARGET, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - if (not move_with_ball) { - move_with_ball = std::make_shared(command_base); - move_with_ball->setParameter("target_x", getParameter("placement_x")); - move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.3); - move_with_ball->setParameter("ball_stabilizing_time", 3.); - move_with_ball->setParameter("reach_threshold", 0.2); - } - - skill_status = move_with_ball->run(visualizer); - command.disablePlacementAvoidance(); - command.disableGoalAreaAvoidance(); - command.disableRuleAreaAvoidance(); - command.setMaxVelocity(0.5); - command.setMaxAcceleration(1.0); - return Status::RUNNING; - }); + addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + if (not move_with_ball) { + move_with_ball = std::make_shared(command_base); + move_with_ball->setParameter("target_x", getParameter("placement_x")); + move_with_ball->setParameter("target_y", getParameter("placement_y")); + move_with_ball->setParameter("dribble_power", 0.3); + move_with_ball->setParameter("ball_stabilizing_time", 3.); + move_with_ball->setParameter("reach_threshold", 0.2); + } + + skill_status = move_with_ball->run(); + command.disablePlacementAvoidance(); + command.disableGoalAreaAvoidance(); + command.disableRuleAreaAvoidance(); + command.setMaxVelocity(0.5); + command.setMaxAcceleration(1.0); + return Status::RUNNING; + }); addTransition( SingleBallPlacementStates::MOVE_TO_TARGET, SingleBallPlacementStates::SLEEP, [this]() { @@ -291,54 +276,50 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() { return skill_status == Status::FAILURE; }); - addStateFunction( - SingleBallPlacementStates::SLEEP, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - if (not sleep) { - sleep = std::make_shared(command_base); - sleep->setParameter("duration", 2.0); - } - skill_status = sleep->run(visualizer); - command.stopHere(); - command.disablePlacementAvoidance(); - command.disableGoalAreaAvoidance(); - command.disableBallAvoidance(); - command.disableRuleAreaAvoidance(); - command.setOmegaLimit(0.0); - return Status::RUNNING; - }); + addStateFunction(SingleBallPlacementStates::SLEEP, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + if (not sleep) { + sleep = std::make_shared(command_base); + sleep->setParameter("duration", 2.0); + } + skill_status = sleep->run(); + command.stopHere(); + command.disablePlacementAvoidance(); + command.disableGoalAreaAvoidance(); + command.disableBallAvoidance(); + command.disableRuleAreaAvoidance(); + command.setOmegaLimit(0.0); + return Status::RUNNING; + }); addTransition(SingleBallPlacementStates::SLEEP, SingleBallPlacementStates::LEAVE_BALL, [this]() { pull_back_angle = robot()->pose.theta; return skill_status == Status::SUCCESS; }); - addStateFunction( - SingleBallPlacementStates::LEAVE_BALL, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); - if (not set_target_position) { - set_target_position = std::make_shared(command_base); - } - // メモ：().normalized() * 0.8したらなぜかゼロベクトルが出来上がってしまう - Vector2 diff = (robot()->pose.pos - world_model()->ball.pos); - diff.normalize(); - diff = diff * 0.8; - auto leave_pos = world_model()->ball.pos + diff; - set_target_position->setParameter("x", leave_pos.x()); - set_target_position->setParameter("y", leave_pos.y()); - set_target_position->setParameter("reach_threshold", 0.05); - - command.setTargetTheta(pull_back_angle); - command.setOmegaLimit(0.0); - command.disablePlacementAvoidance(); - command.disableBallAvoidance(); - command.disableGoalAreaAvoidance(); - command.disableRuleAreaAvoidance(); - skill_status = set_target_position->run(visualizer); - return skill_status; - }); + addStateFunction(SingleBallPlacementStates::LEAVE_BALL, [this]() { + visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + if (not set_target_position) { + set_target_position = std::make_shared(command_base); + } + // メモ：().normalized() * 0.8したらなぜかゼロベクトルが出来上がってしまう + Vector2 diff = (robot()->pose.pos - world_model()->ball.pos); + diff.normalize(); + diff = diff * 0.8; + auto leave_pos = world_model()->ball.pos + diff; + set_target_position->setParameter("x", leave_pos.x()); + set_target_position->setParameter("y", leave_pos.y()); + set_target_position->setParameter("reach_threshold", 0.05); + + command.setTargetTheta(pull_back_angle); + command.setOmegaLimit(0.0); + command.disablePlacementAvoidance(); + command.disableBallAvoidance(); + command.disableGoalAreaAvoidance(); + command.disableRuleAreaAvoidance(); + skill_status = set_target_position->run(); + return skill_status; + }); addTransition( SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, diff --git a/crane_robot_skills/src/sleep.cpp b/crane_robot_skills/src/sleep.cpp index 3c2d06263..32213e71b 100644 --- a/crane_robot_skills/src/sleep.cpp +++ b/crane_robot_skills/src/sleep.cpp @@ -14,7 +14,7 @@ Sleep::Sleep(RobotCommandWrapperBase::SharedPtr & base) setParameter("duration", 0.0); } -Status Sleep::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status Sleep::update() { if (not is_started) { start_time = std::chrono::steady_clock::now(); diff --git a/crane_robot_skills/src/steal_ball.cpp b/crane_robot_skills/src/steal_ball.cpp index a1c019318..407a84cf0 100644 --- a/crane_robot_skills/src/steal_ball.cpp +++ b/crane_robot_skills/src/steal_ball.cpp @@ -16,68 +16,64 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base) // side: 横から押し出すようにボールを奪う setParameter("steal_method", std::string("side")); setParameter("kicker_power", 0.4); - addStateFunction( - StealBallState::MOVE_TO_FRONT, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - // ボールの正面に移動 - // 到着判定すると遅くなるので、敵ロボットにボールが隠されていなかったら次に行ってもいいかも - auto theirs = world_model()->theirs.getAvailableRobots(); - if (not theirs.empty()) { - auto [ball_holder, distance] = - world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); - Point target_pos = world_model()->ball.pos + getNormVec(ball_holder->pose.theta) * 0.3; - command.setTargetPosition(target_pos); - command.lookAtBallFrom(target_pos); - if ((robot()->pose.pos - target_pos).norm() < 0.2) { - skill_state = Status::SUCCESS; - } else { - skill_state = Status::RUNNING; - } - return skill_state; + addStateFunction(StealBallState::MOVE_TO_FRONT, [this]() -> Status { + // ボールの正面に移動 + // 到着判定すると遅くなるので、敵ロボットにボールが隠されていなかったら次に行ってもいいかも + auto theirs = world_model()->theirs.getAvailableRobots(); + if (not theirs.empty()) { + auto [ball_holder, distance] = + world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); + Point target_pos = world_model()->ball.pos + getNormVec(ball_holder->pose.theta) * 0.3; + command.setTargetPosition(target_pos); + command.lookAtBallFrom(target_pos); + if ((robot()->pose.pos - target_pos).norm() < 0.2) { + skill_state = Status::SUCCESS; } else { - return Status::RUNNING; + skill_state = Status::RUNNING; } - }); + return skill_state; + } else { + return Status::RUNNING; + } + }); // 正面に移動したら突っ込んでボールを奪う addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::STEAL, [this]() { return skill_state == Status::SUCCESS; }); - addStateFunction( - StealBallState::STEAL, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - command.disableBallAvoidance(); - command.disableCollisionAvoidance(); - const auto method = getParameter("steal_method"); - auto their_frontier = world_model()->getNearestRobotWithDistanceFromPoint( - world_model()->ball.pos, world_model()->theirs.getAvailableRobots()); - if (method == "front") { - command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); + addStateFunction(StealBallState::STEAL, [this]() -> Status { + command.disableBallAvoidance(); + command.disableCollisionAvoidance(); + const auto method = getParameter("steal_method"); + auto their_frontier = world_model()->getNearestRobotWithDistanceFromPoint( + world_model()->ball.pos, world_model()->theirs.getAvailableRobots()); + if (method == "front") { + command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); + command.setDribblerTargetPosition(world_model()->ball.pos); + command.dribble(0.5); + } else if (method == "side") { + command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); + if (robot()->getDistance(world_model()->ball.pos) < (0.085 - 0.030)) { + command.setDribblerTargetPosition( + world_model()->ball.pos + + getVerticalVec(world_model()->ball.pos - robot()->pose.pos) * 0.3); + // ロボット半径より近くに来れば急回転して刈り取れる + // command.setTargetTheta( + // getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2); + } else { command.setDribblerTargetPosition(world_model()->ball.pos); - command.dribble(0.5); - } else if (method == "side") { - command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); - if (robot()->getDistance(world_model()->ball.pos) < (0.085 - 0.030)) { - command.setDribblerTargetPosition( - world_model()->ball.pos + - getVerticalVec(world_model()->ball.pos - robot()->pose.pos) * 0.3); - // ロボット半径より近くに来れば急回転して刈り取れる - // command.setTargetTheta( - // getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2); - } else { - command.setDribblerTargetPosition(world_model()->ball.pos); - } - if ( - world_model()->getTheirFrontier().has_value() && - robot()->getDistance(world_model()->ball.pos) < - world_model()->getTheirFrontier()->robot->getDistance(world_model()->ball.pos)) { - command.kickWithChip(0.5); - } else { - command.kickStraight(0.5); - } } - return Status::RUNNING; - }); + if ( + world_model()->getTheirFrontier().has_value() && + robot()->getDistance(world_model()->ball.pos) < + world_model()->getTheirFrontier()->robot->getDistance(world_model()->ball.pos)) { + command.kickWithChip(0.5); + } else { + command.kickStraight(0.5); + } + } + return Status::RUNNING; + }); } } // namespace crane::skills diff --git a/crane_robot_skills/src/sub_attacker.cpp b/crane_robot_skills/src/sub_attacker.cpp index c933a5e34..3b5fd9a34 100644 --- a/crane_robot_skills/src/sub_attacker.cpp +++ b/crane_robot_skills/src/sub_attacker.cpp @@ -17,7 +17,7 @@ SubAttacker::SubAttacker(RobotCommandWrapperBase::SharedPtr & base) : SkillBase( setParameter("kicker_power", 0.8); } -Status SubAttacker::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status SubAttacker::update() { auto dpps_points = getDPPSPoints(this->world_model()->ball.pos, 0.25, 64, world_model()); // モード判断 diff --git a/crane_robot_skills/src/teleop.cpp b/crane_robot_skills/src/teleop.cpp index 10ee616d7..4f5eab7d7 100644 --- a/crane_robot_skills/src/teleop.cpp +++ b/crane_robot_skills/src/teleop.cpp @@ -22,7 +22,7 @@ Teleop::Teleop(RobotCommandWrapperBase::SharedPtr & base) std::cout << "joy subscriber created" << std::endl; } -Status Teleop::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status Teleop::update() { rclcpp::spin_some(this->get_node_base_interface()); if (last_joy_msg.buttons.empty()) { diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index b013ad801..e95a4fb9b 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -91,10 +91,12 @@ struct Task class ROSNode : public rclcpp::Node { public: - ROSNode() : Node("crane_commander") + ROSNode() + : Node("crane_commander"), + visualizer(std::make_unique("simple_ai")) { + crane::ConsaiVisualizerBuffer::activate(*this); world_model = std::make_shared(*this); - visualizer = std::make_shared(*this, "simple_ai"); command_base = std::make_shared("simple_ai", 0, world_model); publisher_robot_commands = create_publisher("/control_targets", 10); @@ -134,7 +136,7 @@ class ROSNode : public rclcpp::Node crane_msgs::msg::RobotFeedbackArray robot_feedback_array; - ConsaiVisualizerWrapper::SharedPtr visualizer; + crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; }; class CraneCommander : public QMainWindow diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp index 207cc1c1c..19099234a 100644 --- a/crane_simple_ai/src/crane_commander.cpp +++ b/crane_simple_ai/src/crane_commander.cpp @@ -145,7 +145,7 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U skills::Status task_result; try { - task_result = task.skill->run(ros_node->visualizer, task.parameters); + task_result = task.skill->run(task.parameters); ros_node->latest_msg = task.skill->getRobotCommand(); std::stringstream ss; task.skill->print(ss); 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 c8a9562f8..c992e8e25 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 @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -26,6 +26,7 @@ namespace crane { using VisualizerObjects = consai_visualizer_msgs::msg::Objects; +using VisualizerObjectsArray = consai_visualizer_msgs::msg::ObjectsArray; using Referee = robocup_ssl_msgs::msg::Referee; class VisualizationDataHandler @@ -41,7 +42,7 @@ class VisualizationDataHandler private: rclcpp::Subscription::SharedPtr sub_referee_; - rclcpp::Publisher::SharedPtr pub_vis_objects_; + rclcpp::Publisher::SharedPtr pub_vis_objects_; }; } // 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 6682e63cc..81ea978c8 100644 --- a/crane_world_model_publisher/src/visualization_data_handler.cpp +++ b/crane_world_model_publisher/src/visualization_data_handler.cpp @@ -38,7 +38,7 @@ using RobotId = robocup_ssl_msgs::msg::RobotId; VisualizationDataHandler::VisualizationDataHandler(rclcpp::Node & node) { pub_vis_objects_ = - node.create_publisher("visualizer_objects", rclcpp::SensorDataQoS()); + node.create_publisher("visualizer_objects", rclcpp::SensorDataQoS()); sub_referee_ = node.create_subscription( "referee", 10, std::bind(&VisualizationDataHandler::publish_vis_referee, this, std::placeholders::_1)); @@ -47,11 +47,12 @@ VisualizationDataHandler::VisualizationDataHandler(rclcpp::Node & node) void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geometry_data) { // geometryを描画情報に変換してpublishする - auto vis_objects = std::make_unique(); + auto vis_objects_array = std::make_unique(); - vis_objects->layer = "vision"; - vis_objects->sub_layer = "geometry"; - vis_objects->z_order = 0; + VisualizerObjects vis_objects; + vis_objects.layer = "vision"; + vis_objects.sub_layer = "geometry"; + vis_objects.z_order = 0; for (const auto & field_line : geometry_data.field().field_lines()) { VisLine line; @@ -65,7 +66,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo line.p2.y = field_line.p2().y() * 0.001; // line.caption = field_line.name; - vis_objects->lines.push_back(line); + vis_objects.lines.push_back(line); } for (const auto & field_arc : geometry_data.field().field_arcs()) { @@ -81,7 +82,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo arc.end_angle = field_arc.a2(); // arc.caption = field_arc.name; - vis_objects->arcs.push_back(arc); + vis_objects.arcs.push_back(arc); } // ペナルティマーク @@ -92,11 +93,11 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo point.x = -geometry_data.field().field_length() * 0.001 / 2.0 + 8.0; point.y = 0.0; // point.caption = "penalty_mark_positive"; - vis_objects->points.push_back(point); + vis_objects.points.push_back(point); point.x = -point.x; // point.caption = "penalty_mark_negative"; - vis_objects->points.push_back(point); + vis_objects.points.push_back(point); // フィールドの枠 VisRect rect; @@ -110,19 +111,22 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo rect.height = (geometry_data.field().field_width() + geometry_data.field().boundary_width() * 2) * 0.001; // rect.caption = "wall"; - vis_objects->rects.push_back(rect); + vis_objects.rects.push_back(rect); - pub_vis_objects_->publish(std::move(vis_objects)); + vis_objects_array->objects.push_back(vis_objects); + pub_vis_objects_->publish(std::move(vis_objects_array)); } void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_frame) { const double VELOCITY_ALPHA = 0.5; // tracked_frameを描画情報に変換してpublishする - auto vis_objects = std::make_unique(); - vis_objects->layer = "vision"; - vis_objects->sub_layer = "tracked"; - vis_objects->z_order = 10; // 一番上に描画する + auto vis_objects_array = std::make_unique(); + + VisualizerObjects vis_objects; + vis_objects.layer = "vision"; + vis_objects.sub_layer = "tracked"; + vis_objects.z_order = 10; // 一番上に描画する VisCircle vis_ball; vis_ball.line_color.name = "black"; @@ -135,7 +139,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ } vis_ball.center.x = ball.pos().x(); vis_ball.center.y = ball.pos().y(); - vis_objects->circles.push_back(vis_ball); + vis_objects.circles.push_back(vis_ball); // ボールは小さいのでボールの周りを大きな円で囲う vis_ball.line_color.name = "crimson"; @@ -143,7 +147,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ vis_ball.line_size = 2; vis_ball.radius = 0.8; vis_ball.caption = "ball is here"; - vis_objects->circles.push_back(vis_ball); + vis_objects.circles.push_back(vis_ball); // 速度を描画 if (ball.has_vel()) { @@ -157,7 +161,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ ball_vel.p2.x = ball.pos().x() + ball.vel().x(); ball_vel.p2.y = ball.pos().y() + ball.vel().y(); // ball_vel.caption = std::to_string(vel_norm); - vis_objects->lines.push_back(ball_vel); + vis_objects.lines.push_back(ball_vel); } } @@ -179,7 +183,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ vis_robot.x = robot.pos().x(); vis_robot.y = robot.pos().y(); vis_robot.theta = robot.orientation(); - vis_objects->robots.push_back(vis_robot); + vis_objects.robots.push_back(vis_robot); // 速度を描画 // if (robot.has_vel() && robot.hans_vel_angular()) { @@ -194,7 +198,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ // robot_vel.p2.x = robot.pos().x() + robot.vel().x(); // robot_vel.p2.y = robot.pos().y() + robot.vel().y(); // robot_vel.caption = std::to_string(vel_norm); - // vis_objects->lines.push_back(robot_vel); + // vis_objects.lines.push_back(robot_vel); // // // 角速度 // const double vel_angular_norm = std::fabs(robot.vel_angular[0]); @@ -205,11 +209,12 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ // robot_vel.p2.x = robot.pos().x() + robot.vel_angular(); // robot_vel.p2.y = robot.pos().y(); // robot_vel.caption = std::to_string(vel_angular_norm); - // vis_objects->lines.push_back(robot_vel); + // vis_objects.lines.push_back(robot_vel); // } } - pub_vis_objects_->publish(std::move(vis_objects)); + vis_objects_array->objects.push_back(vis_objects); + pub_vis_objects_->publish(std::move(vis_objects_array)); } auto parse_stage = [](const auto & ref_stage) -> std::string { @@ -351,10 +356,12 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) const std::string COLOR_TEXT_YELLOW = "yellow"; const std::string COLOR_TEXT_WARNING = "red"; - auto vis_objects = std::make_unique(); - vis_objects->layer = "referee"; - vis_objects->sub_layer = "info"; - vis_objects->z_order = 2; + auto vis_objects_array = std::make_unique(); + + VisualizerObjects vis_objects; + vis_objects.layer = "referee"; + vis_objects.sub_layer = "info"; + vis_objects.z_order = 2; // STAGEとCOMMANDを表示 VisAnnotation vis_annotation; @@ -364,7 +371,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = 0.0; vis_annotation.normalized_width = STAGE_COMMAND_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); vis_annotation.text = parse_command(*msg); vis_annotation.color.name = "white"; @@ -372,7 +379,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = TEXT_HEIGHT; vis_annotation.normalized_width = STAGE_COMMAND_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); // 残り時間とACT_TIMEを表示 if (msg->stage_time_left.size() > 0) { @@ -394,7 +401,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = 0.0; vis_annotation.normalized_width = TIMER_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); } if (msg->current_action_time_remaining.size() > 0) { @@ -415,7 +422,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = TEXT_HEIGHT; vis_annotation.normalized_width = TIMER_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); } // ロボット数 @@ -425,7 +432,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = 0.0; vis_annotation.normalized_width = BOTS_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); vis_annotation.color.name = COLOR_TEXT_YELLOW; vis_annotation.text = "YELLOW BOTS: " + std::to_string(msg->yellow.max_allowed_bots[0]); @@ -433,7 +440,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = TEXT_HEIGHT; vis_annotation.normalized_width = BOTS_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); // カード数 vis_annotation.color.name = COLOR_TEXT_BLUE; @@ -443,7 +450,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = 0.0; vis_annotation.normalized_width = CARDS_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); vis_annotation.color.name = COLOR_TEXT_YELLOW; vis_annotation.text = "R: " + std::to_string(msg->yellow.red_cards) + @@ -452,7 +459,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = TEXT_HEIGHT; vis_annotation.normalized_width = CARDS_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); // イエローカードの時間 auto parse_yellow_card_times = [](const auto & yellow_card_times) { @@ -476,7 +483,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = 0.0; vis_annotation.normalized_width = YELLOW_CARD_TIMES_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); vis_annotation.color.name = COLOR_TEXT_YELLOW; vis_annotation.text = parse_yellow_card_times(msg->yellow.yellow_card_times); @@ -484,7 +491,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = TEXT_HEIGHT; vis_annotation.normalized_width = YELLOW_CARD_TIMES_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); // タイムアウト auto parse_timeouts = [](const auto & timeouts, const auto & timeout_time) { @@ -496,7 +503,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = 0.0; vis_annotation.normalized_width = TIMEOUT_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); vis_annotation.color.name = COLOR_TEXT_YELLOW; vis_annotation.text = parse_timeouts(msg->yellow.timeouts, msg->yellow.timeout_time); @@ -504,7 +511,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_annotation.normalized_y = TEXT_HEIGHT; vis_annotation.normalized_width = TIMEOUT_WIDTH; vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects->annotations.push_back(vis_annotation); + vis_objects.annotations.push_back(vis_annotation); // プレイスメント位置 if ( @@ -519,10 +526,11 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) vis_circle.fill_color.name = "aquamarine"; vis_circle.line_size = 1; vis_circle.caption = "placement pos"; - vis_objects->circles.push_back(vis_circle); + vis_objects.circles.push_back(vis_circle); } } - pub_vis_objects_->publish(std::move(vis_objects)); + vis_objects_array->objects.push_back(vis_objects); + pub_vis_objects_->publish(std::move(vis_objects_array)); } } // namespace crane diff --git a/docs/skill.md b/docs/skill.md index 10c7461e6..3f34333aa 100644 --- a/docs/skill.md +++ b/docs/skill.md @@ -38,7 +38,7 @@ class Kick : public SkillBase オプションとしてprint関数を実装することもできる。 ```c++ -Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override +Status update() override { // メンバーのcommandを使ってロボットを動かす Point pos{0,0}; @@ -73,13 +73,13 @@ public: { // ステートごとのupdate関数を登録する addStateFunction(TestState::STATE_1, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + [this]() -> Status { // STATE_1の処理 return Status::RUNNING; }); addStateFunction(TestState::STATE_2, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + [this]() -> Status { // STATE_2の処理 return Status::RUNNING; }); @@ -181,7 +181,7 @@ public: : context_int(getContextReference("context_int")) {} - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override + Status update() override { // コンテキストの値を変更する（普通の変数同様に読み書きしてOK） context_int = 1; 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 d751fed9e..8d2b6252d 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 @@ -44,10 +44,10 @@ class PlannerBase RUNNING, }; - explicit PlannerBase( - const std::string name, WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : name(name), world_model(world_model), visualizer(visualizer) + explicit PlannerBase(const std::string name, WorldModelWrapper::SharedPtr & world_model) + : name(name), + world_model(world_model), + visualizer(std::make_unique("session_planner", name)) { } @@ -81,6 +81,7 @@ class PlannerBase for (const auto & command : robot_commands) { msg.robot_commands.emplace_back(command); } + visualizer->flush(); return msg; } @@ -159,10 +160,10 @@ class PlannerBase virtual std::pair> calculateRobotCommand( const std::vector & robots) = 0; - ConsaiVisualizerWrapper::SharedPtr visualizer; - Status status = Status::RUNNING; + ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + private: std::vector> robot_select_callbacks; }; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp index b704a1d97..d8d818970 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp @@ -32,10 +32,8 @@ class AttackerSkillPlanner : public PlannerBase public: std::shared_ptr skill = nullptr; - COMPOSITION_PUBLIC explicit AttackerSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("AttackerSkill", world_model, visualizer) + COMPOSITION_PUBLIC explicit AttackerSkillPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("AttackerSkill", world_model) { } @@ -53,7 +51,7 @@ class AttackerSkillPlanner : public PlannerBase world_model->ball.pos + world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon(), 3, "red", 0.5, ""); - auto status = skill->run(visualizer); + auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; } } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp index 48b3e187a..c659e84cb 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp @@ -33,10 +33,8 @@ class CatchBallPlanner : public PlannerBase { public: COMPOSITION_PUBLIC - explicit CatchBallPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("catch_ball", world_model, visualizer) + explicit CatchBallPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("catch_ball", world_model) { default_point << -1.0, 0.; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp index fa5227967..5ec841eeb 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp @@ -29,10 +29,8 @@ class DefenderPlanner : public PlannerBase { public: COMPOSITION_PUBLIC - explicit DefenderPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("defender", world_model, visualizer) + explicit DefenderPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("defender", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp index e9e756fb5..62cb9aa71 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp @@ -28,10 +28,8 @@ class FormationPlanner : public PlannerBase { public: COMPOSITION_PUBLIC - explicit FormationPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("formation", world_model, visualizer) + explicit FormationPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("formation", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp index e840e64d9..148caf71a 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp @@ -29,10 +29,8 @@ class MarkerPlanner : public PlannerBase { public: COMPOSITION_PUBLIC - explicit MarkerPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("marker", world_model, visualizer) + explicit MarkerPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("marker", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_free_kick_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_free_kick_planner.hpp index 0138cb428..818b1eb42 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_free_kick_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_free_kick_planner.hpp @@ -36,10 +36,8 @@ class OurDirectFreeKickPlanner : public PlannerBase public: COMPOSITION_PUBLIC - explicit OurDirectFreeKickPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("OurDirectFreeKickPlanner", world_model, visualizer) + explicit OurDirectFreeKickPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("OurDirectFreeKickPlanner", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp index c97b4af4c..f214559ac 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp @@ -32,10 +32,8 @@ class OurKickOffPlanner : public PlannerBase std::shared_ptr kickoff_support; public: - COMPOSITION_PUBLIC explicit OurKickOffPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("our_kickoff_planner", world_model, visualizer) + COMPOSITION_PUBLIC explicit OurKickOffPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("our_kickoff_planner", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp index 5796dea4b..7e6fa3fca 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp @@ -32,10 +32,8 @@ class OurPenaltyKickPlanner : public PlannerBase public: COMPOSITION_PUBLIC - explicit OurPenaltyKickPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("OurPenaltyKickPlanner", world_model, visualizer) + explicit OurPenaltyKickPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("OurPenaltyKickPlanner", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp index 4d36a75a7..39b9154fe 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp @@ -37,9 +37,8 @@ class BallPlacementAvoidancePlanner : public PlannerBase public: COMPOSITION_PUBLIC explicit BallPlacementAvoidancePlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("BallPlacementAvoidance", world_model, visualizer) + WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("BallPlacementAvoidance", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp index 180849c3e..4d6c7198c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp @@ -54,10 +54,8 @@ class SimplePlacerPlanner : public PlannerBase std::unordered_map assignment_map; public: - COMPOSITION_PUBLIC explicit SimplePlacerPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("SimplePlacer", world_model, visualizer) + COMPOSITION_PUBLIC explicit SimplePlacerPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("SimplePlacer", world_model) { const double our_side_sign = world_model->getOurSideSign(); Point p1; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp index f7f233617..4edfe146c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp @@ -30,9 +30,8 @@ namespace crane public: \ std::shared_ptr skill = nullptr; \ COMPOSITION_PUBLIC explicit CLASS_NAME##SkillPlanner( \ - WorldModelWrapper::SharedPtr & world_model, \ - const ConsaiVisualizerWrapper::SharedPtr & visualizer) \ - : PlannerBase(#CLASS_NAME, world_model, visualizer) \ + WorldModelWrapper::SharedPtr & world_model) \ + : PlannerBase(#CLASS_NAME, world_model) \ { \ } \ std::pair> calculateRobotCommand( \ @@ -42,7 +41,7 @@ namespace crane return {PlannerBase::Status::RUNNING, {}}; \ } else { \ std::vector robot_commands; \ - auto status = skill->run(visualizer); \ + auto status = skill->run(); \ return {static_cast(status), {skill->getRobotCommand()}}; \ } \ } \ @@ -66,10 +65,8 @@ class GoalieSkillPlanner : public PlannerBase public: std::shared_ptr skill = nullptr; - COMPOSITION_PUBLIC explicit GoalieSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("Goalie", world_model, visualizer) + COMPOSITION_PUBLIC explicit GoalieSkillPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("Goalie", world_model) { } @@ -94,10 +91,8 @@ class BallPlacementSkillPlanner : public PlannerBase public: std::shared_ptr skill = nullptr; - COMPOSITION_PUBLIC explicit BallPlacementSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("BallPlacement", world_model, visualizer) + COMPOSITION_PUBLIC explicit BallPlacementSkillPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("BallPlacement", world_model) { } @@ -114,10 +109,8 @@ class SubAttackerSkillPlanner : public PlannerBase public: std::shared_ptr skill = nullptr; - COMPOSITION_PUBLIC explicit SubAttackerSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("SubAttacker", world_model, visualizer) + COMPOSITION_PUBLIC explicit SubAttackerSkillPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("SubAttacker", world_model) { } @@ -134,10 +127,8 @@ class StealBallSkillPlanner : public PlannerBase public: std::shared_ptr skill = nullptr; - COMPOSITION_PUBLIC explicit StealBallSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("StealBall", world_model, visualizer) + COMPOSITION_PUBLIC explicit StealBallSkillPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("StealBall", world_model) { } @@ -154,10 +145,8 @@ class FreeKickSaverSkillPlanner : public PlannerBase public: std::shared_ptr skill = nullptr; - COMPOSITION_PUBLIC explicit FreeKickSaverSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("FreeKickSaver", world_model, visualizer) + COMPOSITION_PUBLIC explicit FreeKickSaverSkillPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("FreeKickSaver", world_model) { } @@ -174,10 +163,8 @@ class SimpleKickOffSkillPlanner : public PlannerBase public: std::shared_ptr skill = nullptr; - COMPOSITION_PUBLIC explicit SimpleKickOffSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("SimpleKickOff", world_model, visualizer) + COMPOSITION_PUBLIC explicit SimpleKickOffSkillPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("SimpleKickOff", world_model) { } @@ -195,9 +182,8 @@ class BallNearByPositionerSkillPlanner : public PlannerBase std::vector> skills; COMPOSITION_PUBLIC explicit BallNearByPositionerSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("BallNearByPositionerSkill", world_model, visualizer) + WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("BallNearByPositionerSkill", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp index 1af5da569..fa1e5884c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp @@ -26,10 +26,8 @@ class TemplatePlanner : public PlannerBase { public: COMPOSITION_PUBLIC - explicit TemplatePlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("template", world_model, visualizer) + explicit TemplatePlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("template", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_planner.hpp index 9b98cf66e..2a03abed8 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_planner.hpp @@ -38,10 +38,8 @@ class BallPlacementPlanner : public PlannerBase { public: COMPOSITION_PUBLIC - explicit BallPlacementPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("ball_placement", world_model, visualizer) + explicit BallPlacementPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("ball_placement", world_model) { addRobotSelectCallback([&]() { state = BallPlacementState::START; }); } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp index 3f07dcb88..96f0c2b7d 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/test_planner.hpp @@ -30,10 +30,8 @@ namespace crane class TestPlanner : public PlannerBase { public: - COMPOSITION_PUBLIC explicit TestPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("Test", world_model, visualizer) + COMPOSITION_PUBLIC explicit TestPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("Test", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp index ce42a61e3..99e66c021 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp @@ -33,10 +33,8 @@ class TheirPenaltyKickPlanner : public PlannerBase public: COMPOSITION_PUBLIC - explicit TheirPenaltyKickPlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("TheirPenaltyKickPlanner", world_model, visualizer) + explicit TheirPenaltyKickPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("TheirPenaltyKickPlanner", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp index 75b101894..feb612c6d 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp @@ -40,10 +40,8 @@ class TigersGoaliePlanner : public PlannerBase }; COMPOSITION_PUBLIC - explicit TigersGoaliePlanner( - WorldModelWrapper::SharedPtr & world_model, - const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("tigers_goalie", world_model, visualizer) + explicit TigersGoaliePlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("tigers_goalie", world_model) { } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp index b7df90fa7..de6550c5b 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp @@ -26,10 +26,8 @@ class WaiterPlanner : public PlannerBase { public: COMPOSITION_PUBLIC - explicit WaiterPlanner( - WorldModelWrapper::SharedPtr & world_model, - [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) - : PlannerBase("waiter", world_model, visualizer) + explicit WaiterPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("waiter", world_model) { } diff --git a/session/crane_planner_plugins/src/marker_planner.cpp b/session/crane_planner_plugins/src/marker_planner.cpp index a9f4f90af..d596a4c0d 100644 --- a/session/crane_planner_plugins/src/marker_planner.cpp +++ b/session/crane_planner_plugins/src/marker_planner.cpp @@ -14,7 +14,7 @@ MarkerPlanner::calculateRobotCommand([[maybe_unused]] const std::vector robot_commands; for (auto & [id, skill] : skill_map) { - skill->run(visualizer); + skill->run(); robot_commands.emplace_back(skill->getRobotCommand()); } return {PlannerBase::Status::RUNNING, robot_commands}; diff --git a/session/crane_planner_plugins/src/our_kickoff_planner.cpp b/session/crane_planner_plugins/src/our_kickoff_planner.cpp index d4039a09d..6095ee756 100644 --- a/session/crane_planner_plugins/src/our_kickoff_planner.cpp +++ b/session/crane_planner_plugins/src/our_kickoff_planner.cpp @@ -14,10 +14,10 @@ OurKickOffPlanner::calculateRobotCommand( { std::vector robot_commands; - kickoff_attack->run(visualizer); + kickoff_attack->run(); robot_commands.emplace_back(kickoff_attack->getRobotCommand()); if (kickoff_support) { - kickoff_support->run(visualizer); + kickoff_support->run(); robot_commands.emplace_back(kickoff_support->getRobotCommand()); } diff --git a/session/crane_planner_plugins/src/our_penalty_kick_planner.cpp b/session/crane_planner_plugins/src/our_penalty_kick_planner.cpp index 2c57f5826..2cf4d7bd5 100644 --- a/session/crane_planner_plugins/src/our_penalty_kick_planner.cpp +++ b/session/crane_planner_plugins/src/our_penalty_kick_planner.cpp @@ -24,7 +24,7 @@ OurPenaltyKickPlanner::calculateRobotCommand( robot_commands.push_back(command->getMsg()); } if (kicker) { - auto status = kicker->run(visualizer); + auto status = kicker->run(); robot_commands.emplace_back(kicker->getRobotCommand()); if (status == skills::Status::SUCCESS) { return {Status::SUCCESS, robot_commands}; diff --git a/session/crane_planner_plugins/src/skill_planners.cpp b/session/crane_planner_plugins/src/skill_planners.cpp index 7dec44ce6..ff6ce6dfe 100644 --- a/session/crane_planner_plugins/src/skill_planners.cpp +++ b/session/crane_planner_plugins/src/skill_planners.cpp @@ -15,7 +15,7 @@ GoalieSkillPlanner::calculateRobotCommand( if (not skill) { return {PlannerBase::Status::RUNNING, {}}; } else { - auto status = skill->run(visualizer); + auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; } } @@ -32,7 +32,7 @@ BallPlacementSkillPlanner::calculateRobotCommand( skill->setParameter("placement_x", target->x()); skill->setParameter("placement_y", target->y()); } - auto status = skill->run(visualizer); + auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; } } @@ -71,7 +71,7 @@ SubAttackerSkillPlanner::calculateRobotCommand( if (not skill) { return {PlannerBase::Status::RUNNING, {}}; } else { - auto status = skill->run(visualizer); + auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; } } @@ -120,7 +120,7 @@ StealBallSkillPlanner::calculateRobotCommand( if (not skill) { return {PlannerBase::Status::RUNNING, {}}; } else { - auto status = skill->run(visualizer); + auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; } } @@ -171,7 +171,7 @@ FreeKickSaverSkillPlanner::calculateRobotCommand( if (not skill) { return {PlannerBase::Status::RUNNING, {}}; } else { - auto status = skill->run(visualizer); + auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; } } @@ -204,7 +204,7 @@ SimpleKickOffSkillPlanner::calculateRobotCommand( if (not skill) { return {PlannerBase::Status::RUNNING, {}}; } else { - auto status = skill->run(visualizer); + auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; } } @@ -236,7 +236,7 @@ BallNearByPositionerSkillPlanner::calculateRobotCommand( { std::vector robot_commands(skills.size()); std::transform(skills.begin(), skills.end(), robot_commands.begin(), [&](const auto & skill) { - skill->run(visualizer); + skill->run(); return skill->getRobotCommand(); }); return {PlannerBase::Status::RUNNING, robot_commands}; diff --git a/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp b/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp index a27c0b955..74eb0859b 100644 --- a/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp +++ b/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp @@ -37,7 +37,7 @@ TheirPenaltyKickPlanner::calculateRobotCommand( cmd.disableRuleAreaAvoidance(); cmd.disableGoalAreaAvoidance(); } else { - [[maybe_unused]] auto status = goalie->run(visualizer); + [[maybe_unused]] auto status = goalie->run(); } robot_commands.emplace_back(goalie->getRobotCommand()); } diff --git a/session/crane_session_controller/include/crane_session_controller/session_controller.hpp b/session/crane_session_controller/include/crane_session_controller/session_controller.hpp index 3c7e485c9..187e3b438 100644 --- a/session/crane_session_controller/include/crane_session_controller/session_controller.hpp +++ b/session/crane_session_controller/include/crane_session_controller/session_controller.hpp @@ -47,8 +47,6 @@ class SessionControllerComponent : public rclcpp::Node private: WorldModelWrapper::SharedPtr world_model; - ConsaiVisualizerWrapper::SharedPtr visualizer; - std::deque query_queue; // identifier: situation name, diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index 4be42305d..938175e10 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -24,6 +24,8 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions world_model(std::make_shared(*this)), robot_commands_pub(create_publisher("/control_targets", 1)) { + crane::ConsaiVisualizerBuffer::activate(*this); + world_model->setBallOwnerCalculatorEnabled(true); robot_roles = std::make_shared>(); PlannerBase::robot_roles = robot_roles; @@ -162,8 +164,6 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions declare_parameter("initial_session", "HALT"); auto initial_session = get_parameter("initial_session").as_string(); - visualizer = std::make_shared(*this, "session_controller"); - world_model->addCallback([this, initial_session]() { if (not world_model_ready && not world_model->ours.getAvailableRobotIds().empty()) { world_model_ready = true; @@ -271,7 +271,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions } msg.header.stamp = now(); robot_commands_pub->publish(msg); - visualizer->publish(); + ConsaiVisualizerBuffer::publish(); }); } @@ -315,7 +315,7 @@ void SessionControllerComponent::request( try { const std::unordered_map & prev_roles = *PlannerBase::robot_roles; auto [response, new_planner] = [&]() { - auto planner = generatePlanner(p.session_name, world_model, visualizer); + auto planner = generatePlanner(p.session_name, world_model); auto response = planner->doRobotSelect(req, prev_roles); return std::make_pair(response, planner); }(); diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp index 8ad7350e3..4f0e21a64 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp @@ -7,7 +7,7 @@ #ifndef CRANE_MSG_WRAPPERS__CONSAI_VISUALIZER_WRAPPER_HPP_ #define CRANE_MSG_WRAPPERS__CONSAI_VISUALIZER_WRAPPER_HPP_ -#include +#include #include #include #include @@ -555,284 +555,335 @@ struct ShapeTubeBuilder : public FillShapeColorBuilder SharedPtr; + using ObjectsArray = consai_visualizer_msgs::msg::ObjectsArray; + static inline std::unique_ptr buffer = nullptr; - typedef std::unique_ptr UniquePtr; + rclcpp::Publisher::SharedPtr publisher; - rclcpp::Publisher::SharedPtr publisher; + consai_visualizer_msgs::msg::ObjectsArray message_buffer; - consai_visualizer_msgs::msg::Objects latest_msg; - - ConsaiVisualizerWrapper( - rclcpp::Node & node, const std::string & layer = "default", - const std::string & sub_layer = "default", int z_order = 0) - : publisher( - node.create_publisher("/visualizer_objects", 10)) + template + ConsaiVisualizerBuffer(Node & node, const std::string topic) { - latest_msg.layer = layer; - latest_msg.sub_layer = sub_layer; - latest_msg.z_order = z_order; + publisher = node.template create_publisher(topic, 10); } - void publish(bool clear = true) + template + static auto activate(Node & node, const std::string & topic = "/visualizer_objects") -> void { - publisher->publish(latest_msg); - if (clear) { - latest_msg.annotations.clear(); - latest_msg.points.clear(); - latest_msg.lines.clear(); - latest_msg.arcs.clear(); - latest_msg.rects.clear(); - latest_msg.circles.clear(); - latest_msg.tubes.clear(); - latest_msg.robots.clear(); + if (not active()) { + buffer = std::make_unique(node, topic); } } - void addAnnotation( - const std::string & text, double normed_x, double normed_y, double normed_width, - double normed_height, const std::string & color = "white", double alpha = 1.0) + static auto deactivate() -> void { - consai_visualizer_msgs::msg::ShapeAnnotation annotation; - annotation.text = text; - annotation.normalized_x = normed_x; - annotation.normalized_y = normed_y; - annotation.normalized_width = normed_width; - annotation.normalized_height = normed_height; - annotation.color.name = color; - annotation.color.alpha = alpha; - latest_msg.annotations.push_back(annotation); + if (active()) { + buffer.reset(); + } } - void addAnnotation(consai_visualizer_msgs::msg::ShapeAnnotation annotation) - { - latest_msg.annotations.push_back(annotation); - } + static auto active() -> bool { return buffer != nullptr; } - void addPoint( - double x, double y, int size, const std::string & color = "white", double alpha = 1.0, - const std::string & caption = "") + static auto publish() -> void { - consai_visualizer_msgs::msg::ShapePoint point; - point.x = x; - point.y = y; - point.color.name = color; - point.color.alpha = alpha; - point.size = size; - point.caption = caption; - latest_msg.points.push_back(point); + if (active()) { + buffer->publisher->publish(buffer->message_buffer); + buffer->message_buffer.objects.clear(); + } } - void addPoint( - Point p, int size, const std::string & color = "white", double alpha = 1.0, - const std::string & caption = "") + struct MessageBuilder { - addPoint(p.x(), p.y(), size, color, alpha, caption); - } + using SharedPtr = std::shared_ptr; + using UniquePtr = std::unique_ptr; - void addPoint(consai_visualizer_msgs::msg::ShapePoint point) - { - latest_msg.points.push_back(point); - } + explicit MessageBuilder(const std::string & layer, const std::string & sub_layer = "default") + { + message_buffer.layer = layer; + message_buffer.sub_layer = sub_layer; + } - void addLine( - double x1, double y1, double x2, double y2, int size, const std::string & color = "white", - double alpha = 1.0, const std::string & caption = "") - { - consai_visualizer_msgs::msg::ShapeLine line; - line.p1.x = x1; - line.p1.y = y1; - line.p2.x = x2; - line.p2.y = y2; - line.color.name = color; - line.color.alpha = alpha; - line.size = size; - line.caption = caption; - latest_msg.lines.push_back(line); - } + void clear() + { + message_buffer.annotations.clear(); + message_buffer.points.clear(); + message_buffer.lines.clear(); + message_buffer.arcs.clear(); + message_buffer.rects.clear(); + message_buffer.circles.clear(); + message_buffer.tubes.clear(); + message_buffer.texts.clear(); + message_buffer.robots.clear(); + } - void addLine( - Point p1, Point p2, int size, const std::string & color = "white", double alpha = 1.0, - const std::string & caption = "") - { - addLine(p1.x(), p1.y(), p2.x(), p2.y(), size, color, alpha, caption); - } + void flush() + { + if (ConsaiVisualizerBuffer::active()) { + ConsaiVisualizerBuffer::buffer->message_buffer.objects.push_back(message_buffer); + clear(); + } + } - void addLine(consai_visualizer_msgs::msg::ShapeLine line) { latest_msg.lines.push_back(line); } + using Objects = consai_visualizer_msgs::msg::Objects; + + Objects message_buffer; + + void addAnnotation( + const std::string & text, double normed_x, double normed_y, double normed_width, + double normed_height, const std::string & color = "white", double alpha = 1.0) + { + consai_visualizer_msgs::msg::ShapeAnnotation annotation; + annotation.text = text; + annotation.normalized_x = normed_x; + annotation.normalized_y = normed_y; + annotation.normalized_width = normed_width; + annotation.normalized_height = normed_height; + annotation.color.name = color; + annotation.color.alpha = alpha; + message_buffer.annotations.push_back(annotation); + } - void addArc( - double x, double y, double radius, double start_angle, double end_angle, int size, - const std::string & color = "white", double alpha = 1.0, const std::string & caption = "") - { - consai_visualizer_msgs::msg::ShapeArc arc; - arc.center.x = x; - arc.center.y = y; - arc.radius = radius; - arc.start_angle = start_angle; - arc.end_angle = end_angle; - arc.color.name = color; - arc.color.alpha = alpha; - arc.size = size; - arc.caption = caption; - latest_msg.arcs.push_back(arc); - } + void addAnnotation(consai_visualizer_msgs::msg::ShapeAnnotation annotation) + { + message_buffer.annotations.push_back(annotation); + } - void addArc( - Point center, double radius, double start_angle, double end_angle, int size, - const std::string & color = "white", double alpha = 1.0, const std::string & caption = "") - { - addArc(center.x(), center.y(), radius, start_angle, end_angle, size, color, alpha, caption); - } + void addPoint( + double x, double y, int size, const std::string & color = "white", double alpha = 1.0, + const std::string & caption = "") + { + consai_visualizer_msgs::msg::ShapePoint point; + point.x = x; + point.y = y; + point.color.name = color; + point.color.alpha = alpha; + point.size = size; + point.caption = caption; + message_buffer.points.push_back(point); + } - void addArc(consai_visualizer_msgs::msg::ShapeArc arc) { latest_msg.arcs.push_back(arc); } + void addPoint( + Point p, int size, const std::string & color = "white", double alpha = 1.0, + const std::string & caption = "") + { + addPoint(p.x(), p.y(), size, color, alpha, caption); + } - void addRect( - double x, double y, double width, double height, int line_size, - const std::string & line_color = "white", const std::string & fill_color = "white", - double alpha = 1.0, const std::string & caption = "") - { - consai_visualizer_msgs::msg::ShapeRectangle rect; - rect.center.x = x; - rect.center.y = y; - rect.width = width; - rect.height = height; - rect.line_color.name = line_color; - rect.line_color.alpha = alpha; - rect.fill_color.name = fill_color; - if (fill_color == "") { - rect.fill_color.alpha = 0.0; - } else { - rect.fill_color.alpha = alpha; + void addPoint(consai_visualizer_msgs::msg::ShapePoint point) + { + message_buffer.points.push_back(point); } - rect.line_size = line_size; - rect.caption = caption; - latest_msg.rects.push_back(rect); - } - void addRect( - Point center, double width, double height, int line_size, - const std::string & line_color = "white", const std::string & fill_color = "white", - double alpha = 1.0, const std::string & caption = "") - { - addRect( - center.x(), center.y(), width, height, line_size, line_color, fill_color, alpha, caption); - } + void addLine( + double x1, double y1, double x2, double y2, int size, const std::string & color = "white", + double alpha = 1.0, const std::string & caption = "") + { + consai_visualizer_msgs::msg::ShapeLine line; + line.p1.x = x1; + line.p1.y = y1; + line.p2.x = x2; + line.p2.y = y2; + line.color.name = color; + line.color.alpha = alpha; + line.size = size; + line.caption = caption; + message_buffer.lines.push_back(line); + } - void addRect( - const Box & box, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, const std::string & caption = "") - { - Point center; - bg::centroid(box, center); - const double width = box.max_corner().x() - box.min_corner().x(); - const double height = box.max_corner().y() - box.min_corner().y(); - addRect(center, width, height, line_size, line_color, fill_color, alpha, caption); - } + void addLine( + Point p1, Point p2, int size, const std::string & color = "white", double alpha = 1.0, + const std::string & caption = "") + { + addLine(p1.x(), p1.y(), p2.x(), p2.y(), size, color, alpha, caption); + } - void addRect(consai_visualizer_msgs::msg::ShapeRectangle rect) - { - latest_msg.rects.push_back(rect); - } + void addLine(consai_visualizer_msgs::msg::ShapeLine line) + { + message_buffer.lines.push_back(line); + } - void addCircle( - double x, double y, double radius, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, const std::string & caption = "") - { - consai_visualizer_msgs::msg::ShapeCircle circle; - circle.center.x = x; - circle.center.y = y; - circle.radius = radius; - circle.line_color.name = line_color; - circle.line_color.alpha = alpha; - circle.fill_color.name = fill_color; - if (fill_color == "") { - circle.fill_color.alpha = 0.0; - } else { - circle.fill_color.alpha = alpha; + void addArc( + double x, double y, double radius, double start_angle, double end_angle, int size, + const std::string & color = "white", double alpha = 1.0, const std::string & caption = "") + { + consai_visualizer_msgs::msg::ShapeArc arc; + arc.center.x = x; + arc.center.y = y; + arc.radius = radius; + arc.start_angle = start_angle; + arc.end_angle = end_angle; + arc.color.name = color; + arc.color.alpha = alpha; + arc.size = size; + arc.caption = caption; + message_buffer.arcs.push_back(arc); } - circle.line_size = line_size; - circle.caption = caption; - latest_msg.circles.push_back(circle); - } - void addCircle( - Point center, double radius, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, const std::string & caption = "") - { - addCircle(center.x(), center.y(), radius, line_size, line_color, fill_color, alpha, caption); - } + void addArc( + Point center, double radius, double start_angle, double end_angle, int size, + const std::string & color = "white", double alpha = 1.0, const std::string & caption = "") + { + addArc(center.x(), center.y(), radius, start_angle, end_angle, size, color, alpha, caption); + } - void addCircle(consai_visualizer_msgs::msg::ShapeCircle circle) - { - latest_msg.circles.push_back(circle); - } + void addArc(consai_visualizer_msgs::msg::ShapeArc arc) { message_buffer.arcs.push_back(arc); } + + void addRect( + double x, double y, double width, double height, int line_size, + const std::string & line_color = "white", const std::string & fill_color = "white", + double alpha = 1.0, const std::string & caption = "") + { + consai_visualizer_msgs::msg::ShapeRectangle rect; + rect.center.x = x; + rect.center.y = y; + rect.width = width; + rect.height = height; + rect.line_color.name = line_color; + rect.line_color.alpha = alpha; + rect.fill_color.name = fill_color; + if (fill_color == "") { + rect.fill_color.alpha = 0.0; + } else { + rect.fill_color.alpha = alpha; + } + rect.line_size = line_size; + rect.caption = caption; + message_buffer.rects.push_back(rect); + } - void addTube( - double x1, double y1, double x2, double y2, double radius, int line_size, - const std::string & line_color = "white", const std::string & fill_color = "white", - double alpha = 1.0, const std::string & caption = "") - { - consai_visualizer_msgs::msg::ShapeTube tube; - tube.p1.x = x1; - tube.p1.y = y1; - tube.p2.x = x2; - tube.p2.y = y2; - tube.radius = radius; - tube.line_color.name = line_color; - tube.line_color.alpha = alpha; - tube.fill_color.name = fill_color; - tube.fill_color.alpha = alpha; - tube.line_size = line_size; - tube.caption = caption; - latest_msg.tubes.push_back(tube); - } + void addRect( + Point center, double width, double height, int line_size, + const std::string & line_color = "white", const std::string & fill_color = "white", + double alpha = 1.0, const std::string & caption = "") + { + addRect( + center.x(), center.y(), width, height, line_size, line_color, fill_color, alpha, caption); + } - void addTube( - Point p1, Point p2, double radius, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, const std::string & caption = "") - { - addTube( - p1.x(), p1.y(), p2.x(), p2.y(), radius, line_size, line_color, fill_color, alpha, caption); - } + void addRect( + const Box & box, int line_size, const std::string & line_color = "white", + const std::string & fill_color = "white", double alpha = 1.0, + const std::string & caption = "") + { + Point center; + bg::centroid(box, center); + const double width = box.max_corner().x() - box.min_corner().x(); + const double height = box.max_corner().y() - box.min_corner().y(); + addRect(center, width, height, line_size, line_color, fill_color, alpha, caption); + } - void addTube(consai_visualizer_msgs::msg::ShapeTube tube) { latest_msg.tubes.push_back(tube); } + void addRect(consai_visualizer_msgs::msg::ShapeRectangle rect) + { + message_buffer.rects.push_back(rect); + } - void addRobot( - double id, double x, double y, double theta, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, double line_size = 1, - const std::string & caption = "", bool color_type = false) - { - consai_visualizer_msgs::msg::ShapeRobot robot; - robot.x = x; - robot.y = y; - robot.theta = theta; - robot.radius = 0.09; - robot.line_color.name = line_color; - robot.line_color.alpha = alpha; - robot.fill_color.name = fill_color; - robot.fill_color.alpha = alpha; - robot.line_size = line_size; - robot.caption = caption; - robot.id = id; - robot.color_type = color_type; - latest_msg.robots.push_back(robot); - } + void addCircle( + double x, double y, double radius, int line_size, const std::string & line_color = "white", + const std::string & fill_color = "white", double alpha = 1.0, + const std::string & caption = "") + { + consai_visualizer_msgs::msg::ShapeCircle circle; + circle.center.x = x; + circle.center.y = y; + circle.radius = radius; + circle.line_color.name = line_color; + circle.line_color.alpha = alpha; + circle.fill_color.name = fill_color; + if (fill_color == "") { + circle.fill_color.alpha = 0.0; + } else { + circle.fill_color.alpha = alpha; + } + circle.line_size = line_size; + circle.caption = caption; + message_buffer.circles.push_back(circle); + } - void addRobot( - double id, Point p, double theta, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, double line_size = 1, - const std::string & caption = "", bool color_type = false) - { - addRobot( - id, p.x(), p.y(), theta, line_color, fill_color, alpha, line_size, caption, color_type); - } + void addCircle( + Point center, double radius, int line_size, const std::string & line_color = "white", + const std::string & fill_color = "white", double alpha = 1.0, + const std::string & caption = "") + { + addCircle(center.x(), center.y(), radius, line_size, line_color, fill_color, alpha, caption); + } - void addRobot(consai_visualizer_msgs::msg::ShapeRobot robot) - { - latest_msg.robots.push_back(robot); - } + void addCircle(consai_visualizer_msgs::msg::ShapeCircle circle) + { + message_buffer.circles.push_back(circle); + } + + void addTube( + double x1, double y1, double x2, double y2, double radius, int line_size, + const std::string & line_color = "white", const std::string & fill_color = "white", + double alpha = 1.0, const std::string & caption = "") + { + consai_visualizer_msgs::msg::ShapeTube tube; + tube.p1.x = x1; + tube.p1.y = y1; + tube.p2.x = x2; + tube.p2.y = y2; + tube.radius = radius; + tube.line_color.name = line_color; + tube.line_color.alpha = alpha; + tube.fill_color.name = fill_color; + tube.fill_color.alpha = alpha; + tube.line_size = line_size; + tube.caption = caption; + message_buffer.tubes.push_back(tube); + } + + void addTube( + Point p1, Point p2, double radius, int line_size, const std::string & line_color = "white", + const std::string & fill_color = "white", double alpha = 1.0, + const std::string & caption = "") + { + addTube( + p1.x(), p1.y(), p2.x(), p2.y(), radius, line_size, line_color, fill_color, alpha, caption); + } + + void addTube(consai_visualizer_msgs::msg::ShapeTube tube) + { + message_buffer.tubes.push_back(tube); + } + + void addRobot( + double id, double x, double y, double theta, const std::string & line_color = "white", + const std::string & fill_color = "white", double alpha = 1.0, double line_size = 1, + const std::string & caption = "", bool color_type = false) + { + consai_visualizer_msgs::msg::ShapeRobot robot; + robot.x = x; + robot.y = y; + robot.theta = theta; + robot.radius = 0.09; + robot.line_color.name = line_color; + robot.line_color.alpha = alpha; + robot.fill_color.name = fill_color; + robot.fill_color.alpha = alpha; + robot.line_size = line_size; + robot.caption = caption; + robot.id = id; + robot.color_type = color_type; + message_buffer.robots.push_back(robot); + } + + void addRobot( + double id, Point p, double theta, const std::string & line_color = "white", + const std::string & fill_color = "white", double alpha = 1.0, double line_size = 1, + const std::string & caption = "", bool color_type = false) + { + addRobot( + id, p.x(), p.y(), theta, line_color, fill_color, alpha, line_size, caption, color_type); + } + + void addRobot(consai_visualizer_msgs::msg::ShapeRobot robot) + { + message_buffer.robots.push_back(robot); + } + }; }; } // namespace crane #endif // CRANE_MSG_WRAPPERS__CONSAI_VISUALIZER_WRAPPER_HPP_