Visualizerに送信する描画命令をSVGに変更する (#711)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

.github/workflows/custom_dict.json

@@ -125,6 +125,8 @@
     "libprotobuf",
     "RELWITHDEBINFO",
     "NODEBUG",
-    "remmina"
+    "remmina",
+    "gltf",
+    "svgs"
   ]
 }

consai_ros2/robocup_ssl_msgs/CMakeLists.txt

@@ -47,15 +47,9 @@ add_library(proto_cpp
 target_link_libraries(proto_cpp ${PROTOBUF_LIBRARIES})
 
 # Install
-if("$ENV{ROS_DISTRO}" STRGREATER "galactic")
-  install(
-    FILES ${PROTO_H}
-    DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME})
-else()
 install(
         FILES ${PROTO_H}
-        DESTINATION include/${PROJECT_NAME})
-endif()
+        DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME})
 
 install(
   TARGETS proto_cpp

crane_bringup/launch/crane.launch.py

@@ -58,9 +58,6 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "max_vel", default_value="3.0", description="ロボットの最大速度"
             ),
-            DeclareLaunchArgument(
-                "gui", default_value="true", description="consai_visualizerの起動フラグ"
-            ),
             DeclareLaunchArgument(
                 "speak", default_value="true", description="音声ノードの起動フラグ"
             ),
@@ -215,6 +212,11 @@ def generate_launch_description():
                 # parameters=[{"blue_port": 10311}, {"yellow_port": 10312}],
                 parameters=[{"blue_port": 10301}, {"yellow_port": 10302}],
             ),
+            Node(
+                package="crane_visualization_aggregator",
+                executable="crane_visualization_aggregator_node",
+                output="screen",
+            ),
             Node(
                 package="crane_world_model_publisher",
                 executable="crane_world_model_publisher_node",
@@ -263,12 +265,6 @@ def generate_launch_description():
                     {"voicevox_plugin/volumeScale": 1.0},
                 ],
             ),
-            Node(
-                condition=IfCondition(LaunchConfiguration("gui")),
-                package="consai_visualizer",
-                executable="consai_visualizer",
-                on_exit=default_exit_behavior,
-            ),
             # rosbag recordの起動設定
             GroupAction(
                 condition=IfCondition(LaunchConfiguration("record")),
@@ -279,5 +275,44 @@ def generate_launch_description():
                     ),
                 ],
             ),
+            # https://github.com/foxglove/ros-foxglove-bridge/blob/main/ros2_foxglove_bridge/launch/foxglove_bridge_launch.xml
+            Node(
+                package="foxglove_bridge",
+                executable="foxglove_bridge",
+                parameters=[
+                    {"port": 8765},
+                    {"address": "0.0.0.0"},
+                    {"tls": False},
+                    {"certfile": ""},
+                    {"keyfile": ""},
+                    {"topic_whitelist": [".*"]},
+                    {"service_whitelist": [".*"]},
+                    {"param_whitelist": [".*"]},
+                    {"client_topic_whitelist": [".*"]},
+                    {"min_qos_depth": 1},
+                    {"max_qos_depth": 10},
+                    {"num_threads": 0},
+                    {"send_buffer_limit": 10000000},
+                    {"use_sim_time": False},
+                    {
+                        "capabilities": [
+                            "clientPublish",
+                            "parameters",
+                            "parametersSubscribe",
+                            "services",
+                            "connectionGraph",
+                            "assets",
+                        ]
+                    },
+                    {"include_hidden": False},
+                    {
+                        "asset_uri_allowlist": [
+                            "^package://(?:\\w+/)*\\w+\\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$"
+                        ]
+                    },
+                ],
+                output="screen",
+                on_exit=default_exit_behavior,
+            ),
         ]
     )

crane_bringup/package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>foxglove_bridge</exec_depend>
   <exec_depend>joy</exec_depend>
   <exec_depend>launch_ros</exec_depend>
 

crane_game_analyzer/include/crane_game_analyzer/game_analyzer.hpp

@@ -8,7 +8,7 @@
 #define CRANE_GAME_ANALYZER__GAME_ANALYZER_HPP_
 
 #include <algorithm>
-#include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
+#include <crane_msg_wrappers/crane_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/game_analysis.hpp>
 #include <crane_msgs/msg/world_model.hpp>
@@ -126,7 +126,7 @@ class GameAnalyzerComponent : public rclcpp::Node
 
   GameAnalyzerConfig config;
 
-  ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
+  CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
 };
 }  // namespace crane
 

crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp

@@ -32,7 +32,7 @@ class KickEventDetector
 
   void update(
     const WorldModelWrapper & world_model,
-    const ConsaiVisualizerBuffer::MessageBuilder::UniquePtr & visualizer)
+    const CraneVisualizerBuffer::MessageBuilder::UniquePtr & visualizer)
   {
     {
       Record record;
@@ -59,14 +59,28 @@ class KickEventDetector
     std::optional<KickOrigin> kick_event_origin = std::nullopt;
     for (const auto & id : detected_bots.friends) {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("aaaa"), "Detected friend: " << static_cast<int>(id));
-      visualizer->addCircle(
-        world_model.getOurRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
+      SvgCircleBuilder circle_builder;
+      circle_builder.center(world_model.getOurRobot(id)->pose.pos)
+        .radius(0.5)
+        .stroke("blue")
+        .fill("blue")
+        .strokeWidth(2);
+      visualizer->add(circle_builder.getSvgString());
+      // visualizer->addCircle(
+      //   world_model.getOurRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
       kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{true, id});
     }
     for (const auto & id : detected_bots.enemies) {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("aaaa"), "Detected enemy: " << static_cast<int>(id));
-      visualizer->addCircle(
-        world_model.getTheirRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
+      SvgCircleBuilder circle_builder;
+      circle_builder.center(world_model.getTheirRobot(id)->pose.pos)
+        .radius(0.5)
+        .stroke("blue")
+        .fill("blue")
+        .strokeWidth(2);
+      visualizer->add(circle_builder.getSvgString());
+      // visualizer->addCircle(
+      //   world_model.getTheirRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
       kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{false, id});
     }
 
@@ -78,14 +92,28 @@ class KickEventDetector
         // キック中断判定
         kick_history.emplace_back(ongoing_kick_origin.value(), world_model.ball.pos);
         ongoing_kick_origin = std::nullopt;
-        visualizer->addCircle(world_model.ball.pos, 3.5, 2, "green", "black", 1.0, "EVENT");
+        SvgCircleBuilder circle_builder;
+        circle_builder.center(world_model.ball.pos)
+          .radius(3.5)
+          .stroke("green")
+          .fill("black")
+          .strokeWidth(2);
+        visualizer->add(circle_builder.getSvgString());
+        // visualizer->addCircle(world_model.ball.pos, 3.5, 2, "green", "black", 1.0, "EVENT");
       }
     }
 
     // 進行中のキックを可視化
     if (ongoing_kick_origin.has_value()) {
-      visualizer->addTube(
-        world_model.ball.pos, ongoing_kick_origin.value().position, 0.2, 2, "red", "", 1.0, "KICK");
+      SvgLineBuilder line_builder;
+      line_builder.start(ongoing_kick_origin.value().position)
+        .end(world_model.ball.pos)
+        .stroke("red")
+        .strokeWidth(2);
+      visualizer->add(line_builder.getSvgString());
+      // visualizer->addTube(
+      //   world_model.ball.pos, ongoing_kick_origin.value().position,
+      //   0.2, 2, "red", "", 1.0, "KICK");
     }
 
     // キックの履歴を可視化

crane_game_analyzer/src/crane_game_analyzer.cpp

@@ -15,11 +15,11 @@ namespace crane
 {
 GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options)
 : Node("crane_game_analyzer", options),
-  visualizer(std::make_unique<ConsaiVisualizerBuffer::MessageBuilder>("game_analyzer"))
+  visualizer(std::make_unique<CraneVisualizerBuffer::MessageBuilder>("game_analyzer"))
 {
   RCLCPP_INFO(get_logger(), "GameAnalyzer is constructed.");
 
-  ConsaiVisualizerBuffer::activate(*this);
+  CraneVisualizerBuffer::activate(*this);
 
   world_model = std::make_unique<WorldModelWrapper>(*this);
 
@@ -37,7 +37,7 @@ GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options
         robot_collision_info->relative_velocity);
     }
     visualizer->flush();
-    ConsaiVisualizerBuffer::publish();
+    CraneVisualizerBuffer::publish();
   });
 }
 

crane_local_planner/CMakeLists.txt

@@ -29,7 +29,7 @@ target_precompile_headers(${PROJECT_NAME}_component
     "<crane_basics/pid_controller.hpp>"
     "<crane_msgs/msg/robot_commands.hpp>"
     "<crane_msg_wrappers/world_model_wrapper.hpp>"
-    "<crane_msg_wrappers/consai_visualizer_wrapper.hpp>"
+    "<crane_msg_wrappers/crane_visualizer_wrapper.hpp>"
     "<rclcpp/rclcpp.hpp>"
 )
 

crane_local_planner/include/crane_local_planner/gridmap_planner.hpp

@@ -9,7 +9,7 @@
 
 #include <algorithm>
 #include <crane_basics/pid_controller.hpp>
-#include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
+#include <crane_msg_wrappers/crane_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <functional>

crane_local_planner/include/crane_local_planner/local_planner.hpp

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

crane_local_planner/include/crane_local_planner/planner_base.hpp

@@ -7,7 +7,7 @@
 #ifndef CRANE_LOCAL_PLANNER__PLANNER_BASE_HPP_
 #define CRANE_LOCAL_PLANNER__PLANNER_BASE_HPP_
 
-#include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
+#include <crane_msg_wrappers/crane_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <memory>
@@ -18,14 +18,14 @@ class LocalPlannerBase
 {
 public:
   LocalPlannerBase(const std::string & name, rclcpp::Node & node)
-  : visualizer(std::make_unique<ConsaiVisualizerBuffer::MessageBuilder>("local_planner", name))
+  : visualizer(std::make_unique<CraneVisualizerBuffer::MessageBuilder>("local_planner/" + name))
   {
     world_model = std::make_shared<WorldModelWrapper>(node);
   }
   virtual crane_msgs::msg::RobotCommands calculateRobotCommand(
     const crane_msgs::msg::RobotCommands & msg) = 0;
 
-  ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
+  CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
 
   WorldModelWrapper::SharedPtr world_model;
 };

crane_local_planner/include/crane_local_planner/simple_planner.hpp

@@ -9,7 +9,7 @@
 
 #include <algorithm>
 #include <crane_basics/pid_controller.hpp>
-#include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
+#include <crane_msg_wrappers/crane_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <functional>
@@ -40,9 +40,18 @@ class SimplePlanner : public LocalPlannerBase
     for (auto & command : commands.robot_commands) {
       auto robot = world_model->getOurRobot(command.robot_id);
       if (not command.position_target_mode.empty()) {
-        visualizer->addLine(
-          robot->pose.pos.x(), robot->pose.pos.y(), command.position_target_mode.front().target_x,
-          command.position_target_mode.front().target_y, 1);
+        // visualizer->addLine(
+        //   robot->pose.pos.x(), robot->pose.pos.y(),
+        //   command.position_target_mode.front().target_x,
+        //   command.position_target_mode.front().target_y, 1);
+        SvgLineBuilder line_builder;
+        line_builder.start(robot->pose.pos)
+          .end(
+            command.position_target_mode.front().target_x,
+            command.position_target_mode.front().target_y)
+          .stroke("red")
+          .strokeWidth(2);
+        visualizer->add(line_builder.getSvgString());
       }
       if (command.local_planner_config.max_velocity > MAX_VEL) {
         command.local_planner_config.max_velocity = MAX_VEL;