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 a3c9d3e0d..83964ebac 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 @@ -16,7 +16,7 @@ namespace crane::skills class GetBallContact : public SkillBase<> { public: - explicit GetBallContact(uint8_t id, const std::shared_ptr & world_model); + explicit GetBallContact(uint8_t id, const std::shared_ptr & wm); 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 76bf612d8..90f28cfc7 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 @@ -18,7 +18,7 @@ namespace crane::skills class GoOverBall : public SkillBase<> { public: - explicit GoOverBall(uint8_t id, const std::shared_ptr & world_model); + explicit GoOverBall(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & out) const override { diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp index 63a2692bc..76db6a537 100644 --- a/crane_robot_skills/include/crane_robot_skills/goalie.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goalie.hpp @@ -18,11 +18,11 @@ namespace crane::skills class Goalie : public SkillBase<> { public: - explicit Goalie(uint8_t id, const std::shared_ptr & world_model); + explicit Goalie(uint8_t id, const std::shared_ptr & wm); - void emitBallFromPenaltyArea(crane::RobotCommandWrapper & command); + void emitBallFromPenaltyArea(crane::RobotCommandWrapper::SharedPtr & command); - void inplay(crane::RobotCommandWrapper & command, bool enable_emit = true); + void inplay(crane::RobotCommandWrapper::SharedPtr & command, bool enable_emit = true); 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 83bccf2cd..d18dd765b 100644 --- a/crane_robot_skills/include/crane_robot_skills/idle.hpp +++ b/crane_robot_skills/include/crane_robot_skills/idle.hpp @@ -16,21 +16,17 @@ namespace crane::skills class Idle : public SkillBase<> { public: - explicit Idle(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("Idle", id, world_model, DefaultStates::DEFAULT) + explicit Idle(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("Idle", id, wm, DefaultStates::DEFAULT) { setParameter("stop_by_position", true); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { // TODO(HansRobo): モーターをOFFにするようにしたほうがバッテリーに優しいかも if (getParameter("stop_by_position")) { - command.stopHere(); + command->stopHere(); } else { - command.setVelocity(0., 0.); + command->setVelocity(0., 0.); } return Status::RUNNING; }); diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp index 9b235c644..3c1fe2617 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp @@ -21,27 +21,25 @@ enum class KickoffAttackState { class KickoffAttack : public SkillBase { public: - explicit KickoffAttack(uint8_t id, const std::shared_ptr & world_model) - : SkillBase( - "KickoffAttack", id, world_model, KickoffAttackState::PREPARE_KICKOFF) + explicit KickoffAttack(uint8_t id, const std::shared_ptr & wm) + : SkillBase("KickoffAttack", id, wm, KickoffAttackState::PREPARE_KICKOFF) { setParameter("target_x", 0.0f); setParameter("target_y", 1.0f); setParameter("kick_power", 0.5); addStateFunction( KickoffAttackState::PREPARE_KICKOFF, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + std::cout << "KickoffAttackState::PREPARE_KICKOFF" << std::endl; if (not go_over_ball) { go_over_ball = std::make_shared(robot->id, world_model); + go_over_ball->setCommander(command); 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->setMaxVelocity(0.5); } - go_over_ball_status = go_over_ball->run(command, visualizer); + go_over_ball_status = go_over_ball->run(visualizer); return Status::RUNNING; }); addTransition( @@ -49,15 +47,12 @@ class KickoffAttack : public SkillBase [this]() -> bool { return go_over_ball_status == Status::SUCCESS; }); addStateFunction( - KickoffAttackState::KICKOFF, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxVelocity(0.5); - command.kickStraight(getParameter("kick_power")); - command.setTargetPosition(world_model->ball.pos); - command.setTerminalVelocity(0.5); + KickoffAttackState::KICKOFF, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + std::cout << "KickoffAttackState::KICKOFF" << std::endl; + command->setMaxVelocity(0.5); + command->kickStraight(getParameter("kick_power")); + command->setTargetPosition(world_model->ball.pos); + command->setTerminalVelocity(0.5); if (world_model->ball.vel.norm() > 0.3) { return Status::SUCCESS; } else { 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 8bcbdf137..686b3f17d 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp @@ -16,20 +16,16 @@ namespace crane::skills class KickoffSupport : public SkillBase<> { public: - explicit KickoffSupport(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("KickoffSupport", id, world_model, DefaultStates::DEFAULT) + explicit KickoffSupport(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("KickoffSupport", id, wm, DefaultStates::DEFAULT) { setParameter("target_x", 0.0f); setParameter("target_y", 1.0f); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target(getParameter("target_x"), getParameter("target_y")); - command.setTargetPosition(target); - command.lookAtBallFrom(target); + command->setTargetPosition(target); + command->lookAtBallFrom(target); return Status::RUNNING; }); } diff --git a/crane_robot_skills/include/crane_robot_skills/marker.hpp b/crane_robot_skills/include/crane_robot_skills/marker.hpp index dd3a9bfeb..7f0b47e67 100644 --- a/crane_robot_skills/include/crane_robot_skills/marker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/marker.hpp @@ -21,18 +21,14 @@ namespace crane::skills class Marker : public SkillBase<> { public: - explicit Marker(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("Marker", id, world_model, DefaultStates::DEFAULT) + explicit Marker(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("Marker", id, wm, DefaultStates::DEFAULT) { setParameter("marking_robot_id", 0); setParameter("mark_distance", 0.5); setParameter("mark_mode", "save_goal"); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { auto marked_robot = world_model->getTheirRobot(getParameter("marking_robot_id")); auto enemy_pos = marked_robot->pose.pos; @@ -48,7 +44,7 @@ class Marker : public SkillBase<> } else { throw std::runtime_error("unknown mark mode"); } - command.setTargetPosition(marking_point, target_theta); + command->setTargetPosition(marking_point, target_theta); return Status::RUNNING; }); } diff --git a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp index 3c94b88ca..45dc91e64 100644 --- a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp +++ b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp @@ -21,15 +21,11 @@ class MoveToGeometry : public SkillBase<> public: explicit MoveToGeometry( uint8_t id, Geometry geometry, const std::shared_ptr & world_model) - : SkillBase<>("MoveToGeometry", id, world_model, DefaultStates::DEFAULT), geometry(geometry) + : SkillBase<>("MoveToGeometry", id, wm, DefaultStates::DEFAULT), geometry(geometry) { setParameter("reach_threshold", 0.1); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if ((robot->pose.pos - getTargetPoint()).norm() < getParameter("reach_threshold")) { return Status::SUCCESS; } else { 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 0554f0ecf..a987a8b9a 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 @@ -26,7 +26,7 @@ enum class MoveWithBallStates { SUCCESS, RUNNING, FAILURE }; class MoveWithBall : public SkillBase<> { public: - explicit MoveWithBall(uint8_t id, const std::shared_ptr & world_model); + explicit MoveWithBall(uint8_t id, const std::shared_ptr & wm); Point getTargetPoint(const Point & target_pos) { diff --git a/crane_robot_skills/include/crane_robot_skills/receiver.hpp b/crane_robot_skills/include/crane_robot_skills/receiver.hpp index 720108a4c..116c23612 100644 --- a/crane_robot_skills/include/crane_robot_skills/receiver.hpp +++ b/crane_robot_skills/include/crane_robot_skills/receiver.hpp @@ -40,7 +40,7 @@ class Receiver : public SkillBase<> double score; }; - explicit Receiver(uint8_t id, const std::shared_ptr & world_model); + explicit Receiver(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { os << "[Receiver]"; } 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 d3565567f..287bd479e 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,12 +14,12 @@ namespace crane::skills { -#define DEFINE_SKILL_COMMAND(name) \ - class Cmd##name : public SkillBase<> \ - { \ - public: \ - explicit Cmd##name(uint8_t id, const std::shared_ptr & world_model); \ - void print(std::ostream & os) const override; \ +#define DEFINE_SKILL_COMMAND(name) \ + class Cmd##name : public SkillBase<> \ + { \ + public: \ + explicit Cmd##name(uint8_t id, const std::shared_ptr & wm); \ + void print(std::ostream & os) const override; \ } DEFINE_SKILL_COMMAND(KickWithChip); diff --git a/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp b/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp index 846fadce2..5298af496 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp @@ -18,7 +18,7 @@ namespace crane::skills class SimpleAttacker : public SkillBase<> { public: - explicit SimpleAttacker(uint8_t id, const std::shared_ptr & world_model); + explicit SimpleAttacker(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { diff --git a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp index 802b13fd7..3c1200783 100644 --- a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp +++ b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp @@ -44,7 +44,7 @@ class SingleBallPlacement : public SkillBase Status skill_status = Status::RUNNING; public: - explicit SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model); + explicit SingleBallPlacement(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { 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 32d8c84f7..c71dafba3 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -88,14 +88,14 @@ class SkillInterface { public: SkillInterface( - const std::string & name, uint8_t id, const std::shared_ptr & world_model) - : name(name), world_model(world_model), robot(world_model->getOurRobot(id)) + const std::string & name, uint8_t id, const std::shared_ptr & wm) + : name(name), world_model(wm), robot(world_model->getOurRobot(id)) { } const std::string name; virtual Status run( - RobotCommandWrapper & command, ConsaiVisualizerWrapper::SharedPtr visualizer, + ConsaiVisualizerWrapper::SharedPtr visualizer, std::optional> parameters_opt = std::nullopt) = 0; @@ -107,6 +107,8 @@ class SkillInterface void setParameter(const std::string & key, const std::string & value) { parameters[key] = value; } + virtual crane_msgs::msg::RobotCommand getRobotCommand() = 0; + template auto getParameter(const std::string & key) const { @@ -152,19 +154,27 @@ template class SkillBase : public SkillInterface { public: - using StateFunctionType = std::function &, const std::shared_ptr &, - crane::RobotCommandWrapper &, ConsaiVisualizerWrapper::SharedPtr)>; + using StateFunctionType = std::function; SkillBase( - const std::string & name, uint8_t id, const std::shared_ptr & world_model, - StatesType init_state) - : SkillInterface(name, id, world_model), state_machine(init_state) + const std::string & name, uint8_t id, const std::shared_ptr & wm, + StatesType init_state, const std::shared_ptr & robot_command = nullptr) + : SkillInterface(name, id, wm), state_machine(init_state) + { + if (robot_command) { + command = robot_command; + } else { + command = std::make_shared(id, wm); + } + } + + void setCommander(const std::shared_ptr & commander) { + this->command = commander; } Status run( - RobotCommandWrapper & command, ConsaiVisualizerWrapper::SharedPtr visualizer, + ConsaiVisualizerWrapper::SharedPtr visualizer, std::optional> parameters_opt = std::nullopt) override { @@ -173,14 +183,17 @@ class SkillBase : public SkillInterface } state_machine.update(); - command.latest_msg.current_pose.x = robot->pose.pos.x(); - command.latest_msg.current_pose.y = robot->pose.pos.y(); - command.latest_msg.current_pose.theta = robot->pose.theta; + command->latest_msg.current_pose.x = robot->pose.pos.x(); + command->latest_msg.current_pose.y = robot->pose.pos.y(); + command->latest_msg.current_pose.theta = robot->pose.theta; - return state_functions[state_machine.getCurrentState()]( - world_model, robot, command, visualizer); + return state_functions[state_machine.getCurrentState()](visualizer); } + crane_msgs::msg::RobotCommand getRobotCommand() override { return command->getMsg(); } + + // std::shared_ptr commander() const { return command; } + void addStateFunction(const StatesType & state, StateFunctionType function) { if (state_functions.find(state) != state_functions.end()) { @@ -216,6 +229,8 @@ class SkillBase : public SkillInterface // operator<< がAのprivateメンバにアクセスできるようにfriend宣言 friend std::ostream & operator<<(std::ostream & os, const SkillBase & skill_base); + + std::shared_ptr command = nullptr; }; } // namespace crane::skills diff --git a/crane_robot_skills/include/crane_robot_skills/sleep.hpp b/crane_robot_skills/include/crane_robot_skills/sleep.hpp index 74ad5906b..073418fe6 100644 --- a/crane_robot_skills/include/crane_robot_skills/sleep.hpp +++ b/crane_robot_skills/include/crane_robot_skills/sleep.hpp @@ -16,16 +16,12 @@ namespace crane::skills class Sleep : public SkillBase<> { public: - explicit Sleep(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("Sleep", id, world_model, DefaultStates::DEFAULT) + explicit Sleep(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("Sleep", id, wm, DefaultStates::DEFAULT) { setParameter("duration", 0.0); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not is_started) { start_time = std::chrono::steady_clock::now(); is_started = true; diff --git a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp index 12c5e5689..589d34711 100644 --- a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp +++ b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp @@ -21,8 +21,8 @@ namespace crane::skills class TurnAroundPoint : public SkillBase<> { public: - explicit TurnAroundPoint(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("TurnAroundPoint", id, world_model, DefaultStates::DEFAULT) + explicit TurnAroundPoint(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("TurnAroundPoint", id, wm, DefaultStates::DEFAULT) { setParameter("target_x", 0.0); setParameter("target_y", 0.0); @@ -32,11 +32,7 @@ class TurnAroundPoint : public SkillBase<> setParameter("max_velocity", 0.5); setParameter("max_turn_omega", M_PI_4); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target_point(getParameter("target_x"), getParameter("target_y")); double target_angle = getParameter("target_angle"); if (target_distance < 0.0) { @@ -53,7 +49,7 @@ class TurnAroundPoint : public SkillBase<> } if (std::abs(getAngleDiff(getAngle(robot->pose.pos - target_point), target_angle)) < 0.1) { - command.stopHere(); + command->stopHere(); return Status::SUCCESS; } else { // 円弧を描いて移動する @@ -71,14 +67,14 @@ class TurnAroundPoint : public SkillBase<> (dr * getParameter("dr_p_gain"))) + getNormVec(current_angle + std::copysign(M_PI_2, angle_diff)) * std::min(max_velocity, std::abs(angle_diff * 0.6)); - command.setVelocity(velocity); + command->setVelocity(velocity); // current_target_angle += std::copysign(max_turn_omega / 30.0f, angle_diff); - // command.setTargetPosition( + // command->setTargetPosition( // target_point + getNormVec(current_target_angle) * target_distance); // 中心点の方を向く - command.setTargetTheta(normalizeAngle(current_angle + M_PI)); + command->setTargetTheta(normalizeAngle(current_angle + M_PI)); return Status::RUNNING; } }); diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp index 384e32c75..1eee31205 100644 --- a/crane_robot_skills/src/get_ball_contact.cpp +++ b/crane_robot_skills/src/get_ball_contact.cpp @@ -8,17 +8,13 @@ namespace crane::skills { -GetBallContact::GetBallContact(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("GetBallContact", id, world_model, DefaultStates::DEFAULT) +GetBallContact::GetBallContact(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("GetBallContact", id, wm, DefaultStates::DEFAULT) { setParameter("min_contact_duration", 0.5); setParameter("dribble_power", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { // 規定時間以上接していたらOK std::cout << "ContactDuration: " << std::chrono::duration_cast( @@ -36,9 +32,9 @@ GetBallContact::GetBallContact(uint8_t id, const std::shared_ptrball.pos); - command.setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); - command.dribble(getParameter("dribble_power")); + command->setDribblerTargetPosition(world_model->ball.pos); + command->setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); + command->dribble(getParameter("dribble_power")); return Status::RUNNING; } }); diff --git a/crane_robot_skills/src/go_over_ball.cpp b/crane_robot_skills/src/go_over_ball.cpp index 206c54540..7ddf62340 100644 --- a/crane_robot_skills/src/go_over_ball.cpp +++ b/crane_robot_skills/src/go_over_ball.cpp @@ -8,19 +8,15 @@ namespace crane::skills { -GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("GoOverBall", id, world_model, DefaultStates::DEFAULT) +GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("GoOverBall", id, wm, DefaultStates::DEFAULT) { setParameter("next_target_x", 0.0); setParameter("next_target_y", 0.0); setParameter("margin", 0.5); setParameter("reach_threshold", 0.05); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not has_started) { Point next_target{ getParameter("next_target_x"), getParameter("next_target_y")}; @@ -32,7 +28,7 @@ GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wo has_started = true; } - command.lookAtBallFrom(final_target_pos); + command->lookAtBallFrom(final_target_pos); auto final_distance = (robot->pose.pos - final_target_pos).norm(); auto [intermediate_distance, intermediate_point] = [&]() { @@ -45,17 +41,14 @@ GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wo } }(); - std::cout << "final_distance: " << final_distance << std::endl; - std::cout << "intermediate_distance: " << intermediate_distance << std::endl; - if (intermediate_distance < final_distance && not has_passed_intermediate_target) { - command.setTargetPosition(intermediate_point); + command->setTargetPosition(intermediate_point); if (intermediate_distance < getParameter("reach_threshold")) { std::cout << "Reached intermediate target" << std::endl; has_passed_intermediate_target = true; } } else { - command.setTargetPosition(final_target_pos); + command->setTargetPosition(final_target_pos); } if (final_distance < getParameter("reach_threshold")) { diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 16718e876..c2ed0d8d7 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -8,16 +8,12 @@ namespace crane::skills { -Goalie::Goalie(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("Goalie", id, world_model, DefaultStates::DEFAULT) +Goalie::Goalie(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("Goalie", id, wm, DefaultStates::DEFAULT) { setParameter("run_inplay", true); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { auto situation = world_model->play_situation.getSituationCommandID(); if (getParameter("run_inplay")) { situation = crane_msgs::msg::PlaySituation::INPLAY; @@ -25,7 +21,7 @@ Goalie::Goalie(uint8_t id, const std::shared_ptr & world_mode switch (situation) { case crane_msgs::msg::PlaySituation::HALT: phase = "HALT, stop here"; - command.stopHere(); + command->stopHere(); break; case crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION: [[fallthrough]]; diff --git a/crane_robot_skills/src/move_with_ball.cpp b/crane_robot_skills/src/move_with_ball.cpp index 7db119e69..fd742829f 100644 --- a/crane_robot_skills/src/move_with_ball.cpp +++ b/crane_robot_skills/src/move_with_ball.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("MoveWithBall", id, world_model, DefaultStates::DEFAULT) +MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("MoveWithBall", id, wm, DefaultStates::DEFAULT) { setParameter("target_x", 0.0); setParameter("target_y", 0.0); @@ -26,16 +26,12 @@ MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr setParameter("dribble_target_horizon", 0.2); setParameter("ball_stabilizing_time", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxVelocity(0.5); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxVelocity(0.5); auto target_pos = parseTargetPoint(); - command.setDribblerTargetPosition(target_pos); + command->setDribblerTargetPosition(target_pos); target_theta = getAngle(target_pos - robot->pose.pos); - command.setTargetTheta(target_theta); + command->setTargetTheta(target_theta); // 開始時にボールに接していることが前提にある if (not robot->ball_contact.findPastContact(getParameter("ball_lost_timeout"))) { // ボールが離れたら失敗 @@ -48,17 +44,17 @@ MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr if ( getElapsedSec(*ball_stabilizing_start_time) > getParameter("ball_stabilizing_time")) { - command.dribble(0.0); + command->dribble(0.0); return Status::SUCCESS; } else { return Status::RUNNING; } } else { phase = "目標位置に向かっています"; - command.setTargetPosition(getTargetPoint(target_pos)); + command->setTargetPosition(getTargetPoint(target_pos)); // 目標姿勢の角度ではなく、目標位置の方向を向いて進む - command.setTargetTheta(target_theta); - command.dribble(getParameter("dribble_power")); + command->setTargetTheta(target_theta); + command->dribble(getParameter("dribble_power")); return Status::RUNNING; } }); diff --git a/crane_robot_skills/src/receiver.cpp b/crane_robot_skills/src/receiver.cpp index 68dd2ad76..f45cc882d 100644 --- a/crane_robot_skills/src/receiver.cpp +++ b/crane_robot_skills/src/receiver.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -Receiver::Receiver(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("Receiver", id, world_model, DefaultStates::DEFAULT) +Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("Receiver", id, wm, DefaultStates::DEFAULT) { setParameter("passer_id", 0); setParameter("receive_x", 0.0); @@ -17,17 +17,13 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & world_ setParameter("ball_vel_threshold", 0.5); setParameter("kicker_power", 0.7); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - auto dpps_points = getDPPSPoints(world_model->ball.pos, 0.25, 16); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + auto dpps_points = getDPPSPoints(this->world_model->ball.pos, 0.25, 16); // モード判断 // こちらへ向かう速度成分 float ball_vel = world_model->ball.vel.dot((robot->pose.pos - world_model->ball.pos).normalized()); - command.kickStraight(getParameter("kicker_power")); + command->kickStraight(getParameter("kicker_power")); if (ball_vel > getParameter("ball_vel_threshold")) { Segment ball_line( world_model->ball.pos, @@ -39,7 +35,7 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & world_ // シュートをブロックしない // TODO(HansRobo): これでは延長線上に相手ゴールのあるパスが全くできなくなるので要修正 if (bg::intersects(ball_line, goal_line)) { - command.stopHere(); + command->stopHere(); } else { // ボールの進路上に移動 ClosestPoint result; @@ -51,10 +47,10 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & world_ auto to_goal = getNormVec(goal_angle); auto to_ball = (world_model->ball.pos - result.closest_point).normalized(); double intermediate_angle = getAngle(2 * to_goal + to_ball); - command.setTargetTheta(intermediate_angle); + command->setTargetTheta(intermediate_angle); // キッカーの中心のためのオフセット - command.setTargetPosition( + command->setTargetPosition( result.closest_point - (2 * to_goal + to_ball).normalized() * 0.06); } } else { @@ -87,16 +83,16 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & world_ best_position = dpps_point; } } - command.setTargetPosition(best_position); + command->setTargetPosition(best_position); // target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); } // ゴールとボールの中間方向を向く - Point target_pos{command.latest_msg.target_x.front(), command.latest_msg.target_y.front()}; + Point target_pos{command->latest_msg.target_x.front(), command->latest_msg.target_y.front()}; auto goal_angle = getLargestGoalAngleFromPosition(target_pos); auto to_goal = getNormVec(goal_angle); auto to_ball = (world_model->ball.pos - target_pos).normalized(); - command.setTargetTheta(getAngle(to_goal + to_ball)); + command->setTargetTheta(getAngle(to_goal + to_ball)); return Status::RUNNING; }); diff --git a/crane_robot_skills/src/robot_command_as_skill.cpp b/crane_robot_skills/src/robot_command_as_skill.cpp index 1d5f174ad..e5582c941 100644 --- a/crane_robot_skills/src/robot_command_as_skill.cpp +++ b/crane_robot_skills/src/robot_command_as_skill.cpp @@ -9,33 +9,25 @@ namespace crane::skills { -#define ONE_FRAME_IMPLEMENTATION(name, method) \ - Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & world_model) \ - : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ - { \ - addStateFunction( \ - DefaultStates::DEFAULT, \ - [this]( \ - const std::shared_ptr & world_model, \ - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, \ - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { \ - command.method; \ - return Status::SUCCESS; \ - }); \ - } \ +#define ONE_FRAME_IMPLEMENTATION(name, method) \ + Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & wm) \ + : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ + { \ + addStateFunction( \ + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { \ + command->method; \ + return Status::SUCCESS; \ + }); \ + } \ void Cmd##name::print(std::ostream & os) const {} -CmdKickWithChip::CmdKickWithChip(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdKickWithChip", id, world_model, DefaultStates::DEFAULT) +CmdKickWithChip::CmdKickWithChip(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdKickWithChip", id, wm, DefaultStates::DEFAULT) { setParameter("power", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.kickWithChip(getParameter("power")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->kickWithChip(getParameter("power")); return Status::SUCCESS; }); } @@ -46,17 +38,13 @@ void CmdKickWithChip::print(std::ostream & os) const ; } -CmdKickStraight::CmdKickStraight(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdKickStraight", id, world_model, DefaultStates::DEFAULT) +CmdKickStraight::CmdKickStraight(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdKickStraight", id, wm, DefaultStates::DEFAULT) { setParameter("power", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.kickStraight(getParameter("power")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->kickStraight(getParameter("power")); return Status::SUCCESS; }); } @@ -67,17 +55,13 @@ void CmdKickStraight::print(std::ostream & os) const ; } -CmdDribble::CmdDribble(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdDribble", id, world_model, DefaultStates::DEFAULT) +CmdDribble::CmdDribble(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdDribble", id, wm, DefaultStates::DEFAULT) { setParameter("power", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.dribble(getParameter("power")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->dribble(getParameter("power")); return Status::SUCCESS; }); } @@ -88,18 +72,14 @@ void CmdDribble::print(std::ostream & os) const ; } -CmdSetVelocity::CmdSetVelocity(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetVelocity", id, world_model, DefaultStates::DEFAULT) +CmdSetVelocity::CmdSetVelocity(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetVelocity", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setVelocity(getParameter("x"), getParameter("y")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setVelocity(getParameter("x"), getParameter("y")); return Status::SUCCESS; }); } @@ -110,21 +90,17 @@ void CmdSetVelocity::print(std::ostream & os) const } CmdSetTargetPosition::CmdSetTargetPosition( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetTargetPosition", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetTargetPosition", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); setParameter("reach_threshold", 0.1); setParameter("exit_immediately", false); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.setTargetPosition(target); + command->setTargetPosition(target); if (getParameter("exit_immediately")) { return Status::SUCCESS; } else { @@ -144,21 +120,17 @@ void CmdSetTargetPosition::print(std::ostream & os) const } CmdSetDribblerTargetPosition::CmdSetDribblerTargetPosition( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetDribblerTargetPosition", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetDribblerTargetPosition", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); setParameter("reach_threshold", 0.1); setParameter("exit_immediately", false); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.setDribblerTargetPosition(target); + command->setDribblerTargetPosition(target); if (getParameter("exit_immediately")) { return Status::SUCCESS; } else { @@ -178,18 +150,13 @@ void CmdSetDribblerTargetPosition::print(std::ostream & os) const .norm(); } -CmdSetTargetTheta::CmdSetTargetTheta( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetTargetTheta", id, world_model, DefaultStates::DEFAULT) +CmdSetTargetTheta::CmdSetTargetTheta(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetTargetTheta", id, wm, DefaultStates::DEFAULT) { setParameter("theta", 0.0); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setTargetTheta(getParameter("theta")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setTargetTheta(getParameter("theta")); return Status::SUCCESS; }); } @@ -199,16 +166,12 @@ void CmdSetTargetTheta::print(std::ostream & os) const os << "[CmdSetTargetTheta] theta: " << getParameter("theta"); } -CmdStopHere::CmdStopHere(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdStopHere", id, world_model, DefaultStates::DEFAULT) +CmdStopHere::CmdStopHere(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdStopHere", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.stopHere(); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->stopHere(); return Status::SUCCESS; }); } @@ -227,18 +190,13 @@ ONE_FRAME_IMPLEMENTATION(SetGoalieDefault, setGoalieDefault()) ONE_FRAME_IMPLEMENTATION(EnableBallCenteringControl, enableBallCenteringControl()) ONE_FRAME_IMPLEMENTATION(EnableLocalGoalie, enableLocalGoalie()) -CmdSetMaxVelocity::CmdSetMaxVelocity( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetMaxVelocity", id, world_model, DefaultStates::DEFAULT) +CmdSetMaxVelocity::CmdSetMaxVelocity(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetMaxVelocity", id, wm, DefaultStates::DEFAULT) { setParameter("max_velocity", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxVelocity(getParameter("max_velocity")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxVelocity(getParameter("max_velocity")); return Status::SUCCESS; }); } @@ -249,17 +207,13 @@ void CmdSetMaxVelocity::print(std::ostream & os) const } CmdSetMaxAcceleration::CmdSetMaxAcceleration( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetMaxAcceleration", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetMaxAcceleration", id, wm, DefaultStates::DEFAULT) { setParameter("max_acceleration", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxAcceleration(getParameter("max_acceleration")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxAcceleration(getParameter("max_acceleration")); return Status::SUCCESS; }); } @@ -269,17 +223,13 @@ void CmdSetMaxAcceleration::print(std::ostream & os) const os << "[CmdSetMaxAcceleration] max_acceleration: " << getParameter("max_acceleration"); } -CmdSetMaxOmega::CmdSetMaxOmega(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetMaxOmega", id, world_model, DefaultStates::DEFAULT) +CmdSetMaxOmega::CmdSetMaxOmega(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetMaxOmega", id, wm, DefaultStates::DEFAULT) { setParameter("max_omega", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxOmega(getParameter("max_omega")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxOmega(getParameter("max_omega")); return Status::SUCCESS; }); } @@ -290,17 +240,13 @@ void CmdSetMaxOmega::print(std::ostream & os) const } CmdSetTerminalVelocity::CmdSetTerminalVelocity( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetTerminalVelocity", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetTerminalVelocity", id, wm, DefaultStates::DEFAULT) { setParameter("terminal_velocity", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setTerminalVelocity(getParameter("terminal_velocity")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setTerminalVelocity(getParameter("terminal_velocity")); return Status::SUCCESS; }); } @@ -310,52 +256,37 @@ void CmdSetTerminalVelocity::print(std::ostream & os) const os << "[CmdSetTerminalVelocity] terminal_velocity: " << getParameter("terminal_velocity"); } -CmdEnableStopFlag::CmdEnableStopFlag( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdEnableStopFlag ", id, world_model, DefaultStates::DEFAULT) +CmdEnableStopFlag::CmdEnableStopFlag(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdEnableStopFlag ", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.stopEmergency(true); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->stopEmergency(true); return Status::SUCCESS; }); } void CmdEnableStopFlag::print(std::ostream & os) const { os << "[CmdEnableStopFlag]"; } -CmdDisableStopFlag::CmdDisableStopFlag( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdDisableStopFlag", id, world_model, DefaultStates::DEFAULT) +CmdDisableStopFlag::CmdDisableStopFlag(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdDisableStopFlag", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.stopEmergency(false); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->stopEmergency(false); return Status::SUCCESS; }); } void CmdDisableStopFlag::print(std::ostream & os) const { os << "[CmdDisableStopFlag]"; } -CmdLiftUpDribbler::CmdLiftUpDribbler( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLiftUpDribbler", id, world_model, DefaultStates::DEFAULT) +CmdLiftUpDribbler::CmdLiftUpDribbler(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLiftUpDribbler", id, wm, DefaultStates::DEFAULT) { setParameter("enable", true); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.liftUpDribbler(getParameter("enable")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->liftUpDribbler(getParameter("enable")); return Status::SUCCESS; }); } @@ -365,19 +296,15 @@ void CmdLiftUpDribbler::print(std::ostream & os) const os << "[CmdLiftUpDribbler] enable: " << getParameter("enable"); } -CmdLookAt::CmdLookAt(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLookAt", id, world_model, DefaultStates::DEFAULT) +CmdLookAt::CmdLookAt(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLookAt", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.lookAt(target); + command->lookAt(target); return Status::SUCCESS; }); } @@ -387,36 +314,27 @@ void CmdLookAt::print(std::ostream & os) const os << "[CmdLookAt] x: " << getParameter("x") << " y: " << getParameter("y"); } -CmdLookAtBall::CmdLookAtBall(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLookAtBall", id, world_model, DefaultStates::DEFAULT) +CmdLookAtBall::CmdLookAtBall(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLookAtBall", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.lookAtBall(); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->lookAtBall(); return Status::SUCCESS; }); } void CmdLookAtBall::print(std::ostream & os) const { os << "[CmdLookAtBall]"; } -CmdLookAtBallFrom::CmdLookAtBallFrom( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLookAtBallFrom", id, world_model, DefaultStates::DEFAULT) +CmdLookAtBallFrom::CmdLookAtBallFrom(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLookAtBallFrom", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.lookAtBallFrom(target); + command->lookAtBallFrom(target); return Status::SUCCESS; }); } diff --git a/crane_robot_skills/src/simple_attacker.cpp b/crane_robot_skills/src/simple_attacker.cpp index a830f2f69..14f517695 100644 --- a/crane_robot_skills/src/simple_attacker.cpp +++ b/crane_robot_skills/src/simple_attacker.cpp @@ -8,15 +8,11 @@ namespace crane::skills { -SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("SimpleAttacker", id, world_model, DefaultStates::DEFAULT) +SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("SimpleAttacker", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { auto [best_target, goal_angle_width] = getBestShootTargetWithWidth(); // シュートの隙がないときは仲間へパス @@ -38,22 +34,22 @@ SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptrball.pos); // ボールと敵ゴールの延長線上にいない && 角度があってないときは，中間ポイントを経由 if (dot < 0.95 || std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.05) { - command.setTargetPosition(intermediate_point); - command.enableCollisionAvoidance(); + command->setTargetPosition(intermediate_point); + command->enableCollisionAvoidance(); } else { - command.setTargetPosition(world_model->ball.pos); - command.kickStraight(0.7).disableCollisionAvoidance(); - command.enableCollisionAvoidance(); - command.disableBallAvoidance(); + command->setTargetPosition(world_model->ball.pos); + command->kickStraight(0.7).disableCollisionAvoidance(); + command->enableCollisionAvoidance(); + command->disableBallAvoidance(); } - command.setTargetTheta(getAngle(best_target - world_model->ball.pos)); + command->setTargetTheta(getAngle(best_target - world_model->ball.pos)); bool is_in_defense = world_model->isDefenseArea(world_model->ball.pos); bool is_in_field = world_model->isFieldInside(world_model->ball.pos); if ((not is_in_field) or is_in_defense) { - command.stopHere(); + command->stopHere(); } return Status::RUNNING; }); diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 7709c789d..1e9e446ec 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -9,27 +9,24 @@ namespace crane::skills { -SingleBallPlacement::SingleBallPlacement( - uint8_t id, const std::shared_ptr & world_model) +SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & wm) : SkillBase( - "SingleBallPlacement", id, world_model, SingleBallPlacementStates::GO_OVER_BALL) + "SingleBallPlacement", id, wm, SingleBallPlacementStates::GO_OVER_BALL) { setParameter("placement_x", 0.); setParameter("placement_y", 0.); addStateFunction( SingleBallPlacementStates::GO_OVER_BALL, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not go_over_ball) { go_over_ball = std::make_shared(robot->id, world_model); + go_over_ball->setCommander(command); go_over_ball->setParameter("next_target_x", getParameter("placement_x")); go_over_ball->setParameter("next_target_y", getParameter("placement_y")); go_over_ball->setParameter("margin", 0.4); } - skill_status = go_over_ball->run(command, visualizer); + skill_status = go_over_ball->run(visualizer); return Status::RUNNING; }); @@ -40,15 +37,13 @@ SingleBallPlacement::SingleBallPlacement( addStateFunction( SingleBallPlacementStates::CONTACT_BALL, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not get_ball_contact) { get_ball_contact = std::make_shared(robot->id, world_model); + get_ball_contact->setCommander(command); } - skill_status = get_ball_contact->run(command, visualizer); + skill_status = get_ball_contact->run(visualizer); return Status::RUNNING; }); @@ -59,17 +54,15 @@ SingleBallPlacement::SingleBallPlacement( addStateFunction( SingleBallPlacementStates::MOVE_TO_TARGET, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not move_with_ball) { move_with_ball = std::make_shared(robot->id, world_model); + move_with_ball->setCommander(command); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); } - skill_status = move_with_ball->run(command, visualizer); + skill_status = move_with_ball->run(visualizer); return Status::RUNNING; }); @@ -80,14 +73,12 @@ SingleBallPlacement::SingleBallPlacement( addStateFunction( SingleBallPlacementStates::PLACE_BALL, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not sleep) { sleep = std::make_shared(robot->id, world_model); + sleep->setCommander(command); } - skill_status = sleep->run(command, visualizer); + skill_status = sleep->run(visualizer); return Status::RUNNING; }); @@ -97,15 +88,13 @@ SingleBallPlacement::SingleBallPlacement( addStateFunction( SingleBallPlacementStates::SLEEP, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not sleep) { sleep = std::make_shared(robot->id, world_model); + sleep->setCommander(command); sleep->setParameter("duration", 0.5); } - skill_status = sleep->run(command, visualizer); + skill_status = sleep->run(visualizer); return Status::RUNNING; }); @@ -115,12 +104,10 @@ SingleBallPlacement::SingleBallPlacement( addStateFunction( SingleBallPlacementStates::LEAVE_BALL, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not set_target_position) { set_target_position = std::make_shared(robot->id, world_model); + set_target_position->setCommander(command); } // メモ：().normalized() * 0.6したらなぜかゼロベクトルが出来上がってしまう Vector2 diff = (robot->pose.pos - world_model->ball.pos); @@ -131,7 +118,7 @@ SingleBallPlacement::SingleBallPlacement( set_target_position->setParameter("y", leave_pos.y()); set_target_position->setParameter("reach_threshold", 0.05); - return set_target_position->run(command, visualizer); + return set_target_position->run(visualizer); }); } } // namespace crane::skills diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index 02e335537..59529597c 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -98,7 +98,6 @@ class ROSNode : public rclcpp::Node ROSNode() : Node("crane_commander") { world_model = std::make_shared(*this); - commander = std::make_shared(0, world_model); visualizer = std::make_shared(*this, "simple_ai"); publisher_robot_commands = create_publisher("/control_targets", 10); @@ -112,24 +111,27 @@ class ROSNode : public rclcpp::Node crane_msgs::msg::RobotCommands msg; msg.header = world_model->getMsg().header; msg.is_yellow = world_model->isYellow(); - msg.robot_commands.push_back(commander->getMsg()); + msg.robot_commands.push_back(latest_msg); publisher_robot_commands->publish(msg); }); } void changeID(uint8_t id) { - commander = std::make_shared(id, world_model); + std::make_shared(robot_id, world_model)->stopHere(); + robot_id = id; } ~ROSNode() {} crane::WorldModelWrapper::SharedPtr world_model; - crane::RobotCommandWrapper::SharedPtr commander; + uint8_t robot_id = 0; rclcpp::TimerBase::SharedPtr timer; + crane_msgs::msg::RobotCommand latest_msg; + rclcpp::Publisher::SharedPtr publisher_robot_commands; rclcpp::Subscription::SharedPtr subscription_robot_feedback; diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp index 28724c197..c1649dd41 100644 --- a/crane_simple_ai/src/crane_commander.cpp +++ b/crane_simple_ai/src/crane_commander.cpp @@ -99,7 +99,7 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U feedback.error_info.push_back(2); feedback.error_info.push_back(3); for (const auto & robot_feedback : robot_feedback_array.feedback) { - if (robot_feedback.robot_id == ros_node->commander->getMsg().robot_id) { + if (robot_feedback.robot_id == ros_node->robot_id) { feedback = robot_feedback; break; } @@ -133,14 +133,14 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U } else { auto & task = task_queue_execution.front(); if (task.skill == nullptr) { - task.skill = skill_generators[task.name]( - ros_node->commander->getMsg().robot_id, ros_node->world_model); + task.skill = skill_generators[task.name](ros_node->robot_id, ros_node->world_model); task.start_time = std::chrono::steady_clock::now(); } skills::Status task_result; try { - task_result = task.skill->run(*ros_node->commander, ros_node->visualizer, task.parameters); + task_result = task.skill->run(ros_node->visualizer, task.parameters); + ros_node->latest_msg = task.skill->getRobotCommand(); std::stringstream ss; task.skill->print(ss); ui->logTextBrowser->append(QString::fromStdString(ss.str())); @@ -255,9 +255,7 @@ void CraneCommander::setupROS2() void CraneCommander::on_robotIDSpinBox_valueChanged(int arg1) { ui->logTextBrowser->append(QString::fromStdString("ID changed to " + std::to_string(arg1))); - ros_node->commander->stopHere(); ros_node->changeID(arg1); - ros_node->commander->stopHere(); } // コマンドが変わったらテーブルにデフォルト値を入れる 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 5a5c1dcb3..b55b47d6f 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 @@ -39,10 +39,9 @@ class MarkerPlanner : public PlannerBase { std::vector robot_commands; - for (auto & [id, value] : skill_map) { - auto & [command, skill] = value; - skill->run(*command, visualizer); - robot_commands.emplace_back(command->getMsg()); + for (auto & [id, skill] : skill_map) { + skill->run(visualizer); + robot_commands.emplace_back(skill->getRobotCommand()); } return {PlannerBase::Status::RUNNING, robot_commands}; } @@ -87,12 +86,9 @@ class MarkerPlanner : public PlannerBase selected_robots.push_back(selectable_robots[min_index]); skill_map.emplace( selectable_robots[min_index], - std::make_pair( - std::make_shared(selectable_robots[min_index], world_model), - std::make_shared(selectable_robots[min_index], world_model))); - skill_map[selectable_robots[min_index]].second->setParameter( - "marking_robot_id", enemy_robot->id); - skill_map[selectable_robots[min_index]].second->setParameter("mark_distance", 0.5); + std::make_shared(selectable_robots[min_index], world_model)); + skill_map[selectable_robots[min_index]]->setParameter("marking_robot_id", enemy_robot->id); + skill_map[selectable_robots[min_index]]->setParameter("mark_distance", 0.5); } return selected_robots; @@ -102,9 +98,7 @@ class MarkerPlanner : public PlannerBase // key: ID of our robot in charge, value: ID of the enemy marked robot std::unordered_map marking_target_map; - std::unordered_map< - uint8_t, std::pair, std::shared_ptr>> - skill_map; + std::unordered_map> skill_map; }; } // namespace crane 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 22d74d3ef..66234722c 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 @@ -28,12 +28,8 @@ class OurKickOffPlanner : public PlannerBase private: std::shared_ptr kickoff_attack; - std::shared_ptr attacker_command; - std::shared_ptr kickoff_support; - std::shared_ptr supporter_command; - public: COMPOSITION_PUBLIC explicit OurKickOffPlanner( WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) @@ -46,11 +42,11 @@ class OurKickOffPlanner : public PlannerBase { std::vector robot_commands; - kickoff_attack->run(*attacker_command, visualizer); - kickoff_support->run(*supporter_command, visualizer); + kickoff_attack->run(visualizer); + kickoff_support->run(visualizer); - robot_commands.emplace_back(attacker_command->getMsg()); - robot_commands.emplace_back(supporter_command->getMsg()); + robot_commands.emplace_back(kickoff_attack->getRobotCommand()); + robot_commands.emplace_back(kickoff_support->getRobotCommand()); // いい感じにSUCCESSも返す return {PlannerBase::Status::RUNNING, robot_commands}; @@ -83,9 +79,7 @@ class OurKickOffPlanner : public PlannerBase }); kickoff_attack = std::make_shared(*best_attacker, world_model); - attacker_command = std::make_shared(*best_attacker, world_model); kickoff_support = std::make_shared(*best_supporter, world_model); - supporter_command = std::make_shared(*best_supporter, world_model); return {*best_attacker, *best_supporter}; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp index 53f32d88d..9ef7ec6a6 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp @@ -19,7 +19,6 @@ #include "receive_planner.hpp" #include "skill_planner.hpp" #include "temporary/ball_placement_planner.hpp" -#include "temporary/ball_placement_with_skill_planner.hpp" #include "tigers_goalie_planner.hpp" #include "waiter_planner.hpp" @@ -32,8 +31,6 @@ auto generatePlanner(const std::string & planner_name, Ts... ts) -> PlannerBase: return std::make_unique(ts...); } else if (planner_name == "ball_placement") { return std::make_unique(ts...); - } else if (planner_name == "ball_placement_with_skill") { - return std::make_unique(ts...); } else if (planner_name == "ball_placement_skill") { return std::make_unique(ts...); } else if (planner_name == "defender") { 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 88232bc96..58772e9d0 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 @@ -65,8 +65,6 @@ class GoalieSkillPlanner : public PlannerBase public: std ::shared_ptr skill = nullptr; - std ::shared_ptr robot_command_wrapper = nullptr; - COMPOSITION_PUBLIC explicit GoalieSkillPlanner( WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) : PlannerBase("Goalie", world_model, visualizer) @@ -76,12 +74,12 @@ class GoalieSkillPlanner : public PlannerBase std ::pair> calculateRobotCommand( const std ::vector & robots) override { - if (not skill or not robot_command_wrapper) { + if (not skill) { return {PlannerBase ::Status ::RUNNING, {}}; } else { std ::vector robot_commands; - auto status = skill->run(*robot_command_wrapper, visualizer); - return {static_cast(status), {robot_command_wrapper->getMsg()}}; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; } } auto getSelectedRobots( @@ -89,8 +87,6 @@ class GoalieSkillPlanner : public PlannerBase -> std ::vector override { skill = std ::make_shared(world_model->getOurGoalieId(), world_model); - robot_command_wrapper = - std ::make_shared(world_model->getOurGoalieId(), world_model); return {world_model->getOurGoalieId()}; } }; @@ -100,8 +96,6 @@ class BallPlacementSkillPlanner : public PlannerBase public: std ::shared_ptr skill = nullptr; - std ::shared_ptr robot_command_wrapper = nullptr; - COMPOSITION_PUBLIC explicit BallPlacementSkillPlanner( WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) : PlannerBase("BallPlacement", world_model, visualizer) @@ -111,7 +105,7 @@ class BallPlacementSkillPlanner : public PlannerBase std ::pair> calculateRobotCommand( const std ::vector & robots) override { - if (not skill or not robot_command_wrapper) { + if (not skill) { return {PlannerBase ::Status ::RUNNING, {}}; } else { if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { @@ -119,8 +113,8 @@ class BallPlacementSkillPlanner : public PlannerBase skill->setParameter("placement_y", target->y()); } std ::vector robot_commands; - auto status = skill->run(*robot_command_wrapper, visualizer); - return {static_cast(status), {robot_command_wrapper->getMsg()}}; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; } } @@ -140,9 +134,6 @@ class BallPlacementSkillPlanner : public PlannerBase skill->setParameter("placement_x", target->x()); skill->setParameter("placement_y", target->y()); } - - robot_command_wrapper = - std ::make_shared(selected_robots.front(), world_model); return {selected_robots.front()}; } }; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp deleted file mode 100644 index 39b99d45b..000000000 --- a/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright (c) 2022 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#ifndef CRANE_PLANNER_PLUGINS__TEMPORARY__BALL_PLACEMENT_WITH_SKILL_PLANNER_HPP_ -#define CRANE_PLANNER_PLUGINS__TEMPORARY__BALL_PLACEMENT_WITH_SKILL_PLANNER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "../visibility_control.h" - -namespace crane -{ - -class BallPlacementWithSkillPlanner : public PlannerBase -{ -public: - enum class BallPlacementState { - GO_TO_BALL, - TURN, - GET_BALL_CONTACT, - MOVE_WITH_BALL, - CLEAR_BALL, - }; - - COMPOSITION_PUBLIC - explicit BallPlacementWithSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) - : PlannerBase("ball_placement_with_skill", world_model, visualizer), - state(BallPlacementState::GO_TO_BALL) - { - } - - std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (robots.size() != 1) { - return {}; - } - - placement_target = world_model->getBallPlacementTarget().value(); - - auto robot = world_model->getRobot(robots.front()); - - if (not move_with_ball) { - move_with_ball = std::make_unique(robot->id, world_model); - move_with_ball->setTargetPoint( - placement_target + - (world_model->ball.pos - placement_target).normalized() * robot->center_to_kicker().norm()); - } - - if (not get_ball_contact) { - get_ball_contact = std::make_unique(robot->id, world_model); - } - - crane::RobotCommandWrapper command(robot->id, world_model); - - switch (state) { - case BallPlacementState::GO_TO_BALL: { - command.setTargetPosition( - world_model->ball.pos + (robot->pose.pos - world_model->ball.pos).normalized() * 0.15); - command.setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); - command.setMaxVelocity(2.0); - command.setTerminalVelocity(0.2); - if (auto distance = world_model->getDistanceFromRobotToBall(robot->id); - distance < 0.20 && distance > 0.15) { - state = BallPlacementState::TURN; - } - break; - } - - case BallPlacementState::TURN: { - if (not turn_around_point) { - turn_around_point = std::make_unique(robot->id, world_model); - turn_around_point->setTargetPoint(world_model->ball.pos); - turn_around_point->setTargetAngle(getAngle(world_model->ball.pos - placement_target)); - turn_around_point->setParameter("max_velocity", 1.5); - turn_around_point->setParameter("max_turn_omega", M_PI); - } - if (turn_around_point->run(command, visualizer) == skills::Status::SUCCESS) { - std::cout << "GET_BALL_CONTACT" << std::endl; - state = BallPlacementState::GET_BALL_CONTACT; - turn_around_point = nullptr; - } - command.setMaxVelocity(1.0); - command.setTerminalVelocity(0.5); - break; - } - - case BallPlacementState::GET_BALL_CONTACT: { - if (get_ball_contact->run(command, visualizer) == skills::Status::SUCCESS) { - Point target_point = - placement_target + (world_model->ball.pos - placement_target).normalized() * - robot->center_to_kicker().norm(); - move_with_ball->setTargetPoint(target_point); - move_with_ball_success_count = 0; - state = BallPlacementState::MOVE_WITH_BALL; - } - command.setMaxVelocity(0.5); - break; - } - - case BallPlacementState::MOVE_WITH_BALL: { - auto status = move_with_ball->run(command, visualizer); - command.setMaxVelocity(0.5); - command.setTerminalVelocity(0.1); - // command.setTerminalVelocity( - // std::min(1.0, - // std::max((double)(robot->pose.pos - placement_target).norm() - 0.1, 0.0))); - command.setMaxOmega(M_PI / 2.0); - if (status == skills::Status::FAILURE) { - state = BallPlacementState::GO_TO_BALL; - } else if (status == skills::Status::SUCCESS) { - move_with_ball_success_count++; - if (move_with_ball_success_count >= 20) { - state = BallPlacementState::CLEAR_BALL; - } - } else { - move_with_ball_success_count = 0; - } - break; - } - - case BallPlacementState::CLEAR_BALL: { - command.setTargetPosition( - placement_target + (robot->pose.pos - placement_target).normalized() * 0.6); - command.setMaxVelocity(0.5); - break; - } - } - - std::vector cmd_msgs{command.getMsg()}; - - return {PlannerBase::Status::RUNNING, cmd_msgs}; - } - - auto getSelectedRobots( - uint8_t selectable_robots_num, const std::vector & selectable_robots) - -> std::vector override - { - return this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, [this](const std::shared_ptr & robot) { - // ボールに近いほどスコアが高い - return 100.0 / - std::max(world_model->getSquareDistanceFromRobotToBall({true, robot->id}), 0.01); - }); - } - -private: - BallPlacementState state; - - std::unique_ptr get_ball_contact = nullptr; - - std::unique_ptr move_with_ball = nullptr; - - std::unique_ptr turn_around_point = nullptr; - - Point placement_target = Point(0.0, 0.0); - - int move_with_ball_success_count = 0; -}; - -} // namespace crane - -#endif // CRANE_PLANNER_PLUGINS__TEMPORARY__BALL_PLACEMENT_WITH_SKILL_PLANNER_HPP_