ConsaiVisualizerWrapperを削除してConsaiVisualizerBufferへ (#630)

consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py

@@ -24,7 +24,7 @@
 
 import rclpy
 from ament_index_python.resources import get_resource
-from consai_visualizer_msgs.msg import Objects
+from consai_visualizer_msgs.msg import ObjectsArray
 from consai_visualizer.field_widget import FieldWidget
 from python_qt_binding import loadUi
 from python_qt_binding.QtCore import QPointF, Qt, QTimer
@@ -64,8 +64,8 @@ def __init__(self, context):
         self._widget.field_widget.set_logger(self._logger)
         self._add_visualizer_layer("caption", "caption")
 
-        self._sub_visualize_objects = self._node.create_subscription(
-            Objects,
+        self._sub_visualize_objects_array = self._node.create_subscription(
+            ObjectsArray,
             "visualizer_objects",
             self._callback_visualizer_objects,
             rclpy.qos.qos_profile_sensor_data,
@@ -146,8 +146,9 @@ def _layer_state_changed(self):
 
     def _callback_visualizer_objects(self, msg):
         # ここでレイヤーを更新する
-        self._add_visualizer_layer(msg.layer, msg.sub_layer)
-        self._widget.field_widget.set_visualizer_objects(msg)
+        for objects in msg.objects:
+            self._add_visualizer_layer(objects.layer, objects.sub_layer)
+            self._widget.field_widget.set_visualizer_objects(objects)
 
     def _add_visualizer_layer(self, layer: str, sub_layer: str, state=Qt.Unchecked):
         # レイヤーに重複しないように項目を追加する

consai_ros2/consai_visualizer_msgs/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(std_msgs REQUIRED)
 set(msg_files
   "msg/Color.msg"
   "msg/Objects.msg"
+  "msg/ObjectsArray.msg"
   "msg/Point.msg"
   "msg/ShapeAnnotation.msg"
   "msg/ShapeArc.msg"

consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg

@@ -0,0 +1 @@
+Objects[] objects

crane_local_planner/include/crane_local_planner/local_planner.hpp

@@ -40,6 +40,8 @@ class LocalPlannerComponent : public rclcpp::Node
     declare_parameter("planner", "gridmap");
     auto planner_str = get_parameter("planner").as_string();
 
+    crane::ConsaiVisualizerBuffer::activate(*this);
+
     process_time_pub = create_publisher<std_msgs::msg::Float32>("process_time", 10);
     if (planner_str == "gridmap") {
       planner = std::make_shared<GridMapPlanner>(*this);

crane_local_planner/include/crane_local_planner/planner_base.hpp

@@ -10,21 +10,22 @@
 #include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
+#include <memory>
 
 namespace crane
 {
 class LocalPlannerBase
 {
 public:
   LocalPlannerBase(std::string name, rclcpp::Node & node)
+  : visualizer(std::make_unique<ConsaiVisualizerBuffer::MessageBuilder>("local_planner", name))
   {
     world_model = std::make_shared<WorldModelWrapper>(node);
-    visualizer = std::make_shared<ConsaiVisualizerWrapper>(node, name);
   }
   virtual crane_msgs::msg::RobotCommands calculateRobotCommand(
     const crane_msgs::msg::RobotCommands & msg) = 0;
 
-  ConsaiVisualizerWrapper::SharedPtr visualizer;
+  ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
 
   WorldModelWrapper::SharedPtr world_model;
 };

crane_local_planner/include/crane_local_planner/simple_planner.hpp

@@ -48,7 +48,6 @@ class SimplePlanner : public LocalPlannerBase
         command.local_planner_config.max_velocity = MAX_VEL;
       }
     }
-    visualizer->publish();
     return commands;
   }
 

crane_local_planner/src/crane_local_planner.cpp

@@ -107,7 +107,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
     }
   }
 
-  planner->visualizer->publish();
+  crane::ConsaiVisualizerBuffer::publish();
 }
 }  // namespace crane
 

crane_robot_receiver/src/robot_receiver_node.cpp

@@ -264,8 +264,10 @@ class RobotReceiverNode : public rclcpp::Node
 {
 public:
   explicit RobotReceiverNode(uint8_t robot_num = 10)
-  : rclcpp::Node("robot_receiver_node"), consai_visualizer_wrapper(*this, "robot_feedback")
+  : rclcpp::Node("robot_receiver_node"),
+    visualizer(std::make_unique<crane::ConsaiVisualizerBuffer::MessageBuilder>("robot_receiver"))
   {
+    crane::ConsaiVisualizerBuffer::activate(*this);
     publisher = create_publisher<crane_msgs::msg::RobotFeedbackArray>("/robot_feedback", 10);
 
     for (int i = 0; i < robot_num; i++) {
@@ -331,6 +333,8 @@ class RobotReceiverNode : public rclcpp::Node
         msg.feedback.push_back(robot_feedback_msg);
       }
       publisher->publish(msg);
+      visualizer->flush();
+      crane::ConsaiVisualizerBuffer::publish();
     });
   }
 
@@ -340,7 +344,7 @@ class RobotReceiverNode : public rclcpp::Node
 
   rclcpp::Publisher<crane_msgs::msg::RobotFeedbackArray>::SharedPtr publisher;
 
-  crane::ConsaiVisualizerWrapper consai_visualizer_wrapper;
+  crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
 };
 
 int main(int argc, char * argv[])

crane_robot_skills/include/crane_robot_skills/ball_nearby_positioner.hpp

@@ -18,7 +18,7 @@ class BallNearByPositioner : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override { os << "[BallNearByPositioner]"; }
 };

crane_robot_skills/include/crane_robot_skills/freekick_saver.hpp

@@ -26,7 +26,7 @@ class FreeKickSaver : public SkillBase<RobotCommandWrapperPosition>
   {
   }
 
-  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
+  Status update() override
   {
     auto cmd = std::make_shared<RobotCommandWrapperPosition>(command);
     auto & ball = world_model()->ball.pos;

crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp

@@ -18,7 +18,7 @@ class GetBallContact : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit GetBallContact(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & out) const override;
 

crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp

@@ -20,7 +20,7 @@ class GoOverBall : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit GoOverBall(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & out) const override;
 

crane_robot_skills/include/crane_robot_skills/goal_kick.hpp

@@ -19,7 +19,7 @@ class GoalKick : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit GoalKick(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override { os << "[GoalKick] "; }
 

crane_robot_skills/include/crane_robot_skills/goalie.hpp

@@ -21,11 +21,11 @@ class Goalie : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit Goalie(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
-  void emitBallFromPenaltyArea(const ConsaiVisualizerWrapper::SharedPtr & visualizer);
+  void emitBallFromPenaltyArea();
 
-  void inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr & visualizer);
+  void inplay(bool enable_emit);
 
   void print(std::ostream & os) const override { os << "[Goalie] " << phase; }
 

crane_robot_skills/include/crane_robot_skills/idle.hpp

@@ -18,7 +18,7 @@ class Idle : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit Idle(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("Idle", base) {}
 
-  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
+  Status update() override
   {
     command.stopHere();
     return Status::RUNNING;

crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp

@@ -23,7 +23,7 @@ class KickoffSupport : public SkillBase<RobotCommandWrapperPosition>
     setParameter("target_y", 0.5f);
   }
 
-  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
+  Status update() override
   {
     Point target(getParameter<double>("target_x"), getParameter<double>("target_y"));
     command.lookAtBallFrom(target).setDribblerTargetPosition(target);

crane_robot_skills/include/crane_robot_skills/marker.hpp

@@ -23,7 +23,7 @@ class Marker : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit Marker(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override { os << "[Marker]"; }
 };

crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp

@@ -28,7 +28,7 @@ class MoveWithBall : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit MoveWithBall(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   Point getTargetPoint(const Point & target_pos);
 

crane_robot_skills/include/crane_robot_skills/receive.hpp

@@ -18,11 +18,11 @@ class Receive : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit Receive(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override { os << "[Receive]"; }
 
-  Point getInterceptionPoint(const ConsaiVisualizerWrapper::SharedPtr & visualizer) const;
+  Point getInterceptionPoint() const;
 };
 
 }  // namespace crane::skills

crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp

@@ -14,13 +14,13 @@
 namespace crane::skills
 {
 
-#define DEFINE_SKILL_COMMAND(name, type)                                           \
-  class Cmd##name : public SkillBase<RobotCommandWrapper##type>                    \
-  {                                                                                \
-  public:                                                                          \
-    explicit Cmd##name(RobotCommandWrapperBase::SharedPtr & base);                 \
-    Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override; \
-    void print(std::ostream & os) const override;                                  \
+#define DEFINE_SKILL_COMMAND(name, type)                           \
+  class Cmd##name : public SkillBase<RobotCommandWrapper##type>    \
+  {                                                                \
+  public:                                                          \
+    explicit Cmd##name(RobotCommandWrapperBase::SharedPtr & base); \
+    Status update() override;                                      \
+    void print(std::ostream & os) const override;                  \
   }
 
 DEFINE_SKILL_COMMAND(KickWithChip, Position);

crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp

@@ -24,7 +24,7 @@ class SimpleKickOff : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit SimpleKickOff(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override { os << "[SimpleKickOff]"; }
 

crane_robot_skills/include/crane_robot_skills/skill_base.hpp

@@ -126,6 +126,7 @@ class SkillInterface
     const std::string & name, uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
   : name(name),
     command_base(std::make_shared<RobotCommandWrapperBase>(name, id, wm)),
+    visualizer(std::make_unique<crane::ConsaiVisualizerBuffer::MessageBuilder>("skill", name)),
     target_theta_context(getContextReference<double>("target_theta")),
     dribble_power_context(getContextReference<double>("dribble_power")),
     kick_power_context(getContextReference<double>("kick_power")),
@@ -137,6 +138,7 @@ class SkillInterface
   SkillInterface(const std::string & name, RobotCommandWrapperBase::SharedPtr command)
   : name(name),
     command_base(command),
+    visualizer(std::make_unique<crane::ConsaiVisualizerBuffer::MessageBuilder>("skill", name)),
     target_theta_context(getContextReference<double>("target_theta")),
     dribble_power_context(getContextReference<double>("dribble_power")),
     kick_power_context(getContextReference<double>("kick_power")),
@@ -149,7 +151,6 @@ class SkillInterface
   const std::string name;
 
   virtual Status run(
-    const ConsaiVisualizerWrapper::SharedPtr & visualizer,
     std::optional<std::unordered_map<std::string, ParameterType>> parameters_opt =
       std::nullopt) = 0;
 
@@ -233,6 +234,8 @@ class SkillInterface
 
   std::unordered_map<std::string, ContextType> contexts;
 
+  crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
+
   Status status = Status::RUNNING;
 
   void updateDefaultContexts()
@@ -271,7 +274,6 @@ class SkillBase : public SkillInterface
   }
 
   Status run(
-    const ConsaiVisualizerWrapper::SharedPtr & visualizer,
     std::optional<std::unordered_map<std::string, ParameterType>> parameters_opt =
       std::nullopt) override
   {
@@ -283,12 +285,13 @@ class SkillBase : public SkillInterface
     command_base->latest_msg.current_pose.y = command_base->robot->pose.pos.y();
     command_base->latest_msg.current_pose.theta = command_base->robot->pose.theta;
 
-    auto ret = update(visualizer);
+    auto ret = update();
     updateDefaultContexts();
+    visualizer->flush();
     return ret;
   }
 
-  virtual Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) = 0;
+  virtual Status update() = 0;
 
   crane_msgs::msg::RobotCommand getRobotCommand() override { return command.getMsg(); }
 
@@ -306,7 +309,7 @@ template <typename StatesType, typename DefaultCommandT = RobotCommandWrapperPos
 class SkillBaseWithState : public SkillInterface
 {
 public:
-  using StateFunctionType = std::function<Status(ConsaiVisualizerWrapper::SharedPtr)>;
+  using StateFunctionType = std::function<Status()>;
 
   SkillBaseWithState(
     const std::string & name, uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm,
@@ -325,7 +328,6 @@ class SkillBaseWithState : public SkillInterface
   }
 
   Status run(
-    const ConsaiVisualizerWrapper::SharedPtr & visualizer,
     std::optional<std::unordered_map<std::string, ParameterType>> parameters_opt =
       std::nullopt) override
   {
@@ -339,8 +341,9 @@ class SkillBaseWithState : public SkillInterface
     command_base->latest_msg.current_pose.y = command_base->robot->pose.pos.y();
     command_base->latest_msg.current_pose.theta = command_base->robot->pose.theta;
 
-    auto ret = state_functions[state_machine.getCurrentState()](visualizer);
+    auto ret = state_functions[state_machine.getCurrentState()]();
     updateDefaultContexts();
+    visualizer->flush();
     return ret;
   }
 

crane_robot_skills/include/crane_robot_skills/sleep.hpp

@@ -18,7 +18,7 @@ class Sleep : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit Sleep(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override;
 

crane_robot_skills/include/crane_robot_skills/sub_attacker.hpp

@@ -22,7 +22,7 @@ class SubAttacker : public SkillBase<RobotCommandWrapperPosition>
 public:
   explicit SubAttacker(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override { os << "[SubAttacker]"; }
 

crane_robot_skills/include/crane_robot_skills/teleop.hpp

@@ -20,7 +20,7 @@ class Teleop : public SkillBase<RobotCommandWrapperPosition>, public rclcpp::Nod
 public:
   explicit Teleop(RobotCommandWrapperBase::SharedPtr & base);
 
-  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;
+  Status update() override;
 
   void print(std::ostream & os) const override { os << "[Teleop]"; }