取り敢えずRobotCommandをSkillとして一通り追加

crane_robot_skills/CMakeLists.txt

@@ -17,7 +17,9 @@ find_package(rclcpp_components REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(${PROJECT_NAME}_component SHARED
-  src/robot_skills.cpp)
+  src/robot_skills.cpp
+  src/robot_command_as_skill.cpp
+)
 
 #rclcpp_components_register_nodes(${PROJECT_NAME}_component "crane::WorldModelPublisherComponent")
 

crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp

@@ -0,0 +1,48 @@
+// Copyright (c) 2023 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_ROBOT_SKILLS__ROBOT_COMMAND_AS_SKILL_HPP_
+#define CRANE_ROBOT_SKILLS__ROBOT_COMMAND_AS_SKILL_HPP_
+
+#include <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane
+{
+
+#define DEFINE_SKILL_COMMAND(name)                                                    \
+  class Cmd##name : public SkillBase<>                                                \
+  {                                                                                   \
+  public:                                                                             \
+    explicit Cmd##name(uint8_t id, std::shared_ptr<WorldModelWrapper> & world_model); \
+  }
+
+DEFINE_SKILL_COMMAND(KickWithChip);
+DEFINE_SKILL_COMMAND(KickStraight);
+DEFINE_SKILL_COMMAND(Dribble);
+DEFINE_SKILL_COMMAND(SetVelocity);
+DEFINE_SKILL_COMMAND(SetTargetPosition);
+DEFINE_SKILL_COMMAND(SetDribblerTargetPosition);
+DEFINE_SKILL_COMMAND(SetTargetTheta);
+DEFINE_SKILL_COMMAND(StopHere);
+DEFINE_SKILL_COMMAND(DisablePlacementAvoidance);
+DEFINE_SKILL_COMMAND(EnablePlacementAvoidance);
+DEFINE_SKILL_COMMAND(DisableBallAvoidance);
+DEFINE_SKILL_COMMAND(EnableBallAvoidance);
+DEFINE_SKILL_COMMAND(DisableCollisionAvoidance);
+DEFINE_SKILL_COMMAND(EnableCollisionAvoidance);
+DEFINE_SKILL_COMMAND(DisableGoalAreaAvoidance);
+DEFINE_SKILL_COMMAND(EnableGoalAreaAvoidance);
+DEFINE_SKILL_COMMAND(SetGoalieDefault);
+DEFINE_SKILL_COMMAND(EnableBallCenteringControl);
+DEFINE_SKILL_COMMAND(EnableLocalGoalie);
+DEFINE_SKILL_COMMAND(SetMaxVelocity);
+DEFINE_SKILL_COMMAND(SetMaxAcceleration);
+DEFINE_SKILL_COMMAND(SetMaxOmega);
+DEFINE_SKILL_COMMAND(SetTerminalVelocity);
+
+}  // namespace crane
+#endif  // CRANE_ROBOT_SKILLS__ROBOT_COMMAND_AS_SKILL_HPP_

crane_robot_skills/src/robot_command_as_skill.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) 2024 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.
+
+#include <crane_robot_skills/robot_command_as_skill.hpp>
+
+namespace crane
+{
+
+#define ONE_FRAME_IMPLEMENTATION(name, method)                                       \
+  Cmd##name::Cmd##name(uint8_t id, std::shared_ptr<WorldModelWrapper> & world_model) \
+  : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT)                \
+  {                                                                                  \
+    addStateFunction(                                                                \
+      DefaultStates::DEFAULT,                                                        \
+      [this](                                                                        \
+        const std::shared_ptr<WorldModelWrapper> & world_model,                      \
+        const std::shared_ptr<RobotInfo> & robot,                                    \
+        crane::RobotCommandWrapper & command) -> SkillBase::Status {                 \
+        command.method;                                                              \
+        return SkillBase::Status::SUCCESS;                                           \
+      });                                                                            \
+  }
+
+#define ETERNAL_IMPLEMENTATION(name, method)                                         \
+  Cmd##name::Cmd##name(uint8_t id, std::shared_ptr<WorldModelWrapper> & world_model) \
+  : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT)                \
+  {                                                                                  \
+    addStateFunction(                                                                \
+      DefaultStates::DEFAULT,                                                        \
+      [this](                                                                        \
+        const std::shared_ptr<WorldModelWrapper> & world_model,                      \
+        const std::shared_ptr<RobotInfo> & robot,                                    \
+        crane::RobotCommandWrapper & command) -> SkillBase::Status {                 \
+        command.method;                                                              \
+        return SkillBase::Status::RUNNING;                                           \
+      });                                                                            \
+  }
+
+ONE_FRAME_IMPLEMENTATION(KickWithChip, kickWithChip(0.5))
+ONE_FRAME_IMPLEMENTATION(KickStraight, kickStraight(0.5))
+ONE_FRAME_IMPLEMENTATION(Dribble, dribble(0.5))
+ONE_FRAME_IMPLEMENTATION(SetVelocity, setVelocity(0.0, 0.0))
+ONE_FRAME_IMPLEMENTATION(SetTargetPosition, setTargetPosition(Point(0.0, 0.0)))
+ONE_FRAME_IMPLEMENTATION(SetDribblerTargetPosition, setDribblerTargetPosition(Point(0.0, 0.0)))
+ONE_FRAME_IMPLEMENTATION(SetTargetTheta, setTargetTheta(0.0))
+ONE_FRAME_IMPLEMENTATION(StopHere, stopHere())
+ONE_FRAME_IMPLEMENTATION(DisablePlacementAvoidance, disablePlacementAvoidance())
+ONE_FRAME_IMPLEMENTATION(EnablePlacementAvoidance, enablePlacementAvoidance())
+ONE_FRAME_IMPLEMENTATION(DisableBallAvoidance, disableBallAvoidance())
+ONE_FRAME_IMPLEMENTATION(EnableBallAvoidance, enableBallAvoidance())
+ONE_FRAME_IMPLEMENTATION(DisableCollisionAvoidance, disableCollisionAvoidance())
+ONE_FRAME_IMPLEMENTATION(EnableCollisionAvoidance, enableCollisionAvoidance())
+ONE_FRAME_IMPLEMENTATION(DisableGoalAreaAvoidance, disableGoalAreaAvoidance())
+ONE_FRAME_IMPLEMENTATION(EnableGoalAreaAvoidance, enableGoalAreaAvoidance())
+ONE_FRAME_IMPLEMENTATION(SetGoalieDefault, setGoalieDefault())
+ONE_FRAME_IMPLEMENTATION(EnableBallCenteringControl, enableBallCenteringControl())
+ONE_FRAME_IMPLEMENTATION(EnableLocalGoalie, enableLocalGoalie())
+ONE_FRAME_IMPLEMENTATION(SetMaxVelocity, setMaxVelocity(0.0))
+ONE_FRAME_IMPLEMENTATION(SetMaxAcceleration, setMaxAcceleration(0.0))
+ONE_FRAME_IMPLEMENTATION(SetMaxOmega, setMaxOmega(0.0))
+ONE_FRAME_IMPLEMENTATION(SetTerminalVelocity, setTerminalVelocity(0.0))
+
+}  // namespace crane