Merge remote-tracking branch 'origin/develop' into feature/aggressive…

…_goalie # Conflicts: # crane_robot_skills/include/crane_robot_skills/goalie.hpp
ibis-ssl · Feb 21, 2024 · fde7ad8 · fde7ad8
2 parents 1058db0 + d6a6307
commit fde7ad8
Show file tree

Hide file tree

Showing 31 changed files with 261 additions and 602 deletions.
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
@@ -16,7 +16,7 @@ namespace crane::skills
 class GetBallContact : public SkillBase<>
 {
 public:
-  explicit GetBallContact(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
+  explicit GetBallContact(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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
@@ -18,7 +18,7 @@ namespace crane::skills
 class GoOverBall : public SkillBase<>
 {
 public:
-  explicit GoOverBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
+  explicit GoOverBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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
@@ -18,11 +18,11 @@ namespace crane::skills
 class Goalie : public SkillBase<>
 {
 public:
-  explicit Goalie(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
+  explicit Goalie(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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
@@ -16,21 +16,17 @@ namespace crane::skills
 class Idle : public SkillBase<>
 {
 public:
-  explicit Idle(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
-  : SkillBase<>("Idle", id, world_model, DefaultStates::DEFAULT)
+  explicit Idle(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
+  : SkillBase<>("Idle", id, wm, DefaultStates::DEFAULT)
   {
     setParameter("stop_by_position", true);
     addStateFunction(
-      DefaultStates::DEFAULT,
-      [this](
-        const std::shared_ptr<WorldModelWrapper> & world_model,
-        const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
-        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
+      DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
         // TODO(HansRobo): モーターをOFFにするようにしたほうがバッテリーに優しいかも
         if (getParameter<bool>("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
@@ -21,43 +21,38 @@ enum class KickoffAttackState {
 class KickoffAttack : public SkillBase<KickoffAttackState>
 {
 public:
-  explicit KickoffAttack(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
-  : SkillBase<KickoffAttackState>(
-      "KickoffAttack", id, world_model, KickoffAttackState::PREPARE_KICKOFF)
+  explicit KickoffAttack(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
+  : SkillBase<KickoffAttackState>("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<WorldModelWrapper> & world_model,
-        const std::shared_ptr<RobotInfo> & 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<GoOverBall>(robot->id, world_model);
+          go_over_ball->setCommander(command);
           go_over_ball->setParameter("next_target_x", getParameter<double>("target_x"));
           go_over_ball->setParameter("next_target_y", getParameter<double>("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(
       KickoffAttackState::PREPARE_KICKOFF, KickoffAttackState::KICKOFF,
       [this]() -> bool { return go_over_ball_status == Status::SUCCESS; });
 
     addStateFunction(
-      KickoffAttackState::KICKOFF,
-      [this](
-        const std::shared_ptr<WorldModelWrapper> & world_model,
-        const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
-        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
-        command.setMaxVelocity(0.5);
-        command.kickStraight(getParameter<double>("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<double>("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
@@ -16,20 +16,16 @@ namespace crane::skills
 class KickoffSupport : public SkillBase<>
 {
 public:
-  explicit KickoffSupport(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
-  : SkillBase<>("KickoffSupport", id, world_model, DefaultStates::DEFAULT)
+  explicit KickoffSupport(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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<WorldModelWrapper> & world_model,
-        const std::shared_ptr<RobotInfo> & robot, RobotCommandWrapper & command,
-        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
+      DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
         Point target(getParameter<double>("target_x"), getParameter<double>("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
@@ -21,18 +21,14 @@ namespace crane::skills
 class Marker : public SkillBase<>
 {
 public:
-  explicit Marker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
-  : SkillBase<>("Marker", id, world_model, DefaultStates::DEFAULT)
+  explicit Marker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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<WorldModelWrapper> & world_model,
-        const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
-        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
+      DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
         auto marked_robot = world_model->getTheirRobot(getParameter<int>("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
@@ -21,15 +21,11 @@ class MoveToGeometry : public SkillBase<>
 public:
   explicit MoveToGeometry(
     uint8_t id, Geometry geometry, const std::shared_ptr<WorldModelWrapper> & 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<WorldModelWrapper> & world_model,
-        const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
-        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
+      DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
         if ((robot->pose.pos - getTargetPoint()).norm() < getParameter<double>("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
@@ -26,7 +26,7 @@ enum class MoveWithBallStates { SUCCESS, RUNNING, FAILURE };
 class MoveWithBall : public SkillBase<>
 {
 public:
-  explicit MoveWithBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
+  explicit MoveWithBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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
@@ -40,7 +40,7 @@ class Receiver : public SkillBase<>
     double score;
   };
 
-  explicit Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
+  explicit Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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
@@ -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<WorldModelWrapper> & 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<WorldModelWrapper> & 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
@@ -18,7 +18,7 @@ namespace crane::skills
 class SimpleAttacker : public SkillBase<>
 {
 public:
-  explicit SimpleAttacker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
+  explicit SimpleAttacker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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
@@ -44,7 +44,7 @@ class SingleBallPlacement : public SkillBase<SingleBallPlacementStates>
   Status skill_status = Status::RUNNING;
 
 public:
-  explicit SingleBallPlacement(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
+  explicit SingleBallPlacement(uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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
@@ -88,14 +88,14 @@ class SkillInterface
 {
 public:
   SkillInterface(
-    const std::string & name, uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
-  : name(name), world_model(world_model), robot(world_model->getOurRobot(id))
+    const std::string & name, uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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<std::unordered_map<std::string, ParameterType>> 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 <class T>
   auto getParameter(const std::string & key) const
   {
@@ -152,19 +154,27 @@ template <typename StatesType = DefaultStates>
 class SkillBase : public SkillInterface
 {
 public:
-  using StateFunctionType = std::function<Status(
-    const std::shared_ptr<WorldModelWrapper> &, const std::shared_ptr<RobotInfo> &,
-    crane::RobotCommandWrapper &, ConsaiVisualizerWrapper::SharedPtr)>;
+  using StateFunctionType = std::function<Status(ConsaiVisualizerWrapper::SharedPtr)>;
 
   SkillBase(
-    const std::string & name, uint8_t id, const std::shared_ptr<WorldModelWrapper> & 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<WorldModelWrapper> & wm,
+    StatesType init_state, const std::shared_ptr<RobotCommandWrapper> & robot_command = nullptr)
+  : SkillInterface(name, id, wm), state_machine(init_state)
+  {
+    if (robot_command) {
+      command = robot_command;
+    } else {
+      command = std::make_shared<RobotCommandWrapper>(id, wm);
+    }
+  }
+
+  void setCommander(const std::shared_ptr<RobotCommandWrapper> & commander)
   {
+    this->command = commander;
   }
 
   Status run(
-    RobotCommandWrapper & command, ConsaiVisualizerWrapper::SharedPtr visualizer,
+    ConsaiVisualizerWrapper::SharedPtr visualizer,
     std::optional<std::unordered_map<std::string, ParameterType>> 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<RobotCommandWrapper> 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<StatesType> & skill_base);
+
+  std::shared_ptr<RobotCommandWrapper> 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
@@ -16,16 +16,12 @@ namespace crane::skills
 class Sleep : public SkillBase<>
 {
 public:
-  explicit Sleep(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
-  : SkillBase<>("Sleep", id, world_model, DefaultStates::DEFAULT)
+  explicit Sleep(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
+  : SkillBase<>("Sleep", id, wm, DefaultStates::DEFAULT)
   {
     setParameter("duration", 0.0);
     addStateFunction(
-      DefaultStates::DEFAULT,
-      [this](
-        const std::shared_ptr<WorldModelWrapper> & world_model,
-        const std::shared_ptr<RobotInfo> & 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;