SimpleAIのバックエンドとしてSimpleAIPlannerを実装する

consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py

@@ -128,6 +128,7 @@ def __init__(self, context):
         self.latest_battery_voltage = [0] * 16
 
         # self._widget.pushButton.clicked.connect(self.publish)
+        self._widget.session_injection_comboBox.addItem("simple_ai")
         self._widget.session_injection_comboBox.addItem("HALT")
         self._widget.session_injection_comboBox.addItem("STOP")
         self._widget.session_injection_comboBox.addItem("OUR_KICKOFF_PREPARATION")
@@ -162,6 +163,8 @@ def __init__(self, context):
         self._widget.session_injection_comboBox.addItem("STOP_PRE_OUR_DIRECT_FREE")
         self._widget.session_injection_comboBox.addItem("STOP_PRE_THEIR_DIRECT_FREE")
         self._widget.session_injection_comboBox.addItem("STOP_PRE_FORCE_START")
+        self._widget.session_injection_comboBox.addItem("formation")
+        self._widget.session_injection_comboBox.addItem("TEST")
         self._widget.session_injection_pushButton.clicked.connect(
             self._session_injection
         )

crane_bringup/launch/crane.launch.py

@@ -73,7 +73,6 @@ def generate_launch_description():
                 "record", default_value="false", description="rosbag記録フラグ"
             ),
             Node(
-                condition=UnlessCondition(LaunchConfiguration("simple_ai")),
                 package="crane_session_controller",
                 executable="crane_session_controller_node",
                 output="screen",

crane_msgs/CMakeLists.txt

@@ -30,6 +30,7 @@ set(msg_files
   "msg/analysis/PingStatusArray.msg"
   "msg/analysis/PlaySituation.msg"
   "msg/analysis/FieldAnalysis.msg"
+  "msg/analysis/NamedBool.msg"
   "msg/analysis/NamedFloat.msg"
   "msg/analysis/NamedInt.msg"
   "msg/analysis/NamedPosition.msg"
@@ -57,9 +58,14 @@ set(srv_files
   "srv/RobotSelect.srv"
 )
 
+set(action_files
+  "action/SkillExecution.action"
+)
+
 rosidl_generate_interfaces(${PROJECT_NAME}
   ${msg_files}
   ${srv_files}
+  ${action_files}
   DEPENDENCIES builtin_interfaces std_msgs geometry_msgs
 )
 

crane_msgs/action/SkillExecution.action

@@ -0,0 +1,7 @@
+uint8 robot_id
+string name
+NamedValueArray parameter
+---
+int16 result
+---
+string message

crane_msgs/msg/analysis/NamedBool.msg

@@ -0,0 +1,2 @@
+string name
+bool value

crane_msgs/msg/analysis/NamedValueArray.msg

@@ -1,3 +1,4 @@
+NamedBool[] bool_values
 NamedInt[] int_values
 NamedFloat[] float_values
 NamedString[] string_values

crane_robot_skills/src/teleop.cpp

@@ -13,13 +13,8 @@ Teleop::Teleop(RobotCommandWrapperBase::SharedPtr & base)
 {
   setParameter("rotation_deg", 0.);
   setParameter("use_local_coordinate", false);
-  std::cout << "Teleop skill created" << std::endl;
   joystick_subscription = this->create_subscription<sensor_msgs::msg::Joy>(
-    "/joy", 10, [this](const sensor_msgs::msg::Joy & msg) {
-      std::cout << "joy message received" << std::endl;
-      last_joy_msg = msg;
-    });
-  std::cout << "joy subscriber created" << std::endl;
+    "/joy", 10, [this](const sensor_msgs::msg::Joy & msg) { last_joy_msg = msg; });
 }
 
 Status Teleop::update()

crane_simple_ai/CMakeLists.txt

@@ -42,7 +42,7 @@ ament_auto_add_executable(${PROJECT_NAME}
 )
 
 # Use the Widgets module from Qt 5
-target_link_libraries(${PROJECT_NAME} Qt5::Core Qt5::Gui Qt5::Widgets)
+target_link_libraries(${PROJECT_NAME} Qt5::Core Qt5::Gui Qt5::Widgets glog)
 
 if (BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)

crane_simple_ai/include/crane_commander.hpp

@@ -20,6 +20,7 @@
 #include <cmath>
 #include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/robot_command_wrapper.hpp>
+#include <crane_msgs/action/skill_execution.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <crane_msgs/msg/robot_feedback_array.hpp>
 #include <crane_robot_skills/skill_base.hpp>
@@ -29,6 +30,7 @@
 #include <memory>
 #include <queue>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
 #include <string>
 #include <unordered_map>
 #include <variant>
@@ -64,7 +66,7 @@ struct Task
 
   std::unordered_map<std::string, skills::ParameterType> parameters;
 
-  std::shared_ptr<skills::SkillInterface> skill = nullptr;
+  // std::shared_ptr<skills::SkillInterface> skill = nullptr;
 
   double retry_time = -1.0;
 
@@ -95,41 +97,14 @@ class ROSNode : public rclcpp::Node
   {
     crane::ConsaiVisualizerBuffer::activate(*this);
     world_model = std::make_shared<crane::WorldModelWrapper>(*this);
-    command_base = std::make_shared<RobotCommandWrapperBase>("simple_ai", 0, world_model);
-    publisher_robot_commands =
-      create_publisher<crane_msgs::msg::RobotCommands>("/control_targets", 10);
 
     subscription_robot_feedback = create_subscription<crane_msgs::msg::RobotFeedbackArray>(
       "/robot_feedback", 10,
       [&](const crane_msgs::msg::RobotFeedbackArray & msg) { robot_feedback_array = msg; });
-
-    timer = rclcpp::create_timer(this, get_clock(), std::chrono::milliseconds(33), [&]() {
-      crane_msgs::msg::RobotCommands msg;
-      msg.header = world_model->getMsg().header;
-      msg.is_yellow = world_model->isYellow();
-      msg.on_positive_half = world_model->onPositiveHalf();
-      msg.robot_commands.push_back(latest_msg);
-      publisher_robot_commands->publish(msg);
-    });
-  }
-
-  void changeID(uint8_t id)
-  {
-    auto command = std::make_shared<crane::RobotCommandWrapperPosition>(command_base);
-    command->stopHere();
-    command_base->changeID(id);
   }
 
   crane::WorldModelWrapper::SharedPtr world_model;
 
-  crane::RobotCommandWrapperBase::SharedPtr command_base;
-
-  rclcpp::TimerBase::SharedPtr timer;
-
-  crane_msgs::msg::RobotCommand latest_msg;
-
-  rclcpp::Publisher<crane_msgs::msg::RobotCommands>::SharedPtr publisher_robot_commands;
-
   rclcpp::Subscription<crane_msgs::msg::RobotFeedbackArray>::SharedPtr subscription_robot_feedback;
 
   crane_msgs::msg::RobotFeedbackArray robot_feedback_array;
@@ -140,6 +115,7 @@ class ROSNode : public rclcpp::Node
 
 class CraneCommander : public QMainWindow
 {
+  using SkillExecution = crane_msgs::action::SkillExecution;
   Q_OBJECT
 
 public:
@@ -151,16 +127,14 @@ class CraneCommander : public QMainWindow
 
   void finishROS2() { rclcpp::shutdown(); }
 
-private slots:
-  void on_commandAddPushButton_clicked();
-
-  void on_executionPushButton_clicked();
+  Task createSkillTask();
 
+private slots:
   void on_commandComboBox_currentTextChanged(const QString & command_name);
 
   void on_robotIDSpinBox_valueChanged(int arg1);
 
-  void on_queueClearPushButton_clicked();
+  void on_executionCheckBox_stateChanged(int state);
 
 private:
   void onQueueToBeEmpty();
@@ -171,22 +145,19 @@ private slots:
 private:
   Ui::CraneCommander * ui;
 
-  QTimer ros_update_timer;
-
-  QTimer task_execution_timer;
-
   std::shared_ptr<ROSNode> ros_node;
 
-  std::deque<Task> task_queue;
+  std::unordered_map<std::string, Task> default_task_dict;
+
+  uint8_t robot_id = 0;
 
-  std::deque<Task> task_queue_execution;
+  std::optional<Task> task = std::nullopt;
 
-  std::unordered_map<
-    std::string, std::function<std::shared_ptr<skills::SkillInterface>(
-                   RobotCommandWrapperBase::SharedPtr & base)>>
-    skill_generators;
+  void postSkill(
+    const std::string & name,
+    const std::unordered_map<std::string, skills::ParameterType> & parameters);
 
-  std::unordered_map<std::string, Task> default_task_dict;
+  rclcpp_action::Client<SkillExecution>::SharedPtr skill_execution_client;
 };
 }  // namespace crane
 

crane_simple_ai/package.xml

@@ -13,11 +13,13 @@
   <depend>crane_msgs</depend>
   <depend>crane_robot_skills</depend>
   <depend>crane_visualization_interfaces</depend>
+  <depend>libgoogle-glog-dev</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>
   <depend>qtbase5-dev</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>std_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>