From d4f0e613899e601eb4fe249081e6e667c7fb7837 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 00:21:29 +0900
Subject: [PATCH 01/14] =?UTF-8?q?proto2ros=E3=82=92=E4=BE=9D=E5=AD=98?=
 =?UTF-8?q?=E3=81=AB=E8=BF=BD=E5=8A=A0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 dependency_jazzy.repos | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/dependency_jazzy.repos b/dependency_jazzy.repos
index 1a27dc7cd..05e8fb29b 100644
--- a/dependency_jazzy.repos
+++ b/dependency_jazzy.repos
@@ -7,3 +7,7 @@ repositories:
         type: git
         url: https://github.com/HansRobo/speak_ros_voicevox_plugin.git
         version: main
+    proto2ros:
+        type: git
+        url: https://github.com/ibis-ssl/proto2ros.git
+        version: jazzy

From e314670539c30eb10020b3ab4fe038d5debdf053 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 00:28:38 +0900
Subject: [PATCH 02/14] =?UTF-8?q?consai=5Fvisualizer=5Fmsgs=E3=82=92proto?=
 =?UTF-8?q?=E3=81=8B=E3=82=89=E7=94=9F=E6=88=90?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../consai_visualizer_msgs/CMakeLists.txt     | 130 ++++++++++++++----
 .../consai_visualizer_msgs/__init__.py        |   0
 .../manual_conversions.hpp                    |  10 ++
 .../consai_visualizer_msgs/msg/Color.msg      |   8 --
 .../consai_visualizer_msgs/msg/Objects.msg    |  17 ---
 .../msg/ObjectsArray.msg                      |   1 -
 .../consai_visualizer_msgs/msg/Point.msg      |   4 -
 .../msg/ShapeAnnotation.msg                   |   8 --
 .../consai_visualizer_msgs/msg/ShapeArc.msg   |   8 --
 .../msg/ShapeCircle.msg                       |   7 -
 .../consai_visualizer_msgs/msg/ShapeLine.msg  |   6 -
 .../consai_visualizer_msgs/msg/ShapePoint.msg |   7 -
 .../msg/ShapeRectangle.msg                    |   8 --
 .../consai_visualizer_msgs/msg/ShapeRobot.msg |  14 --
 .../consai_visualizer_msgs/msg/ShapeText.msg  |   7 -
 .../consai_visualizer_msgs/msg/ShapeTube.msg  |   8 --
 .../consai_visualizer_msgs/package.xml        |   5 +
 .../src/manual_conversions.cpp                |   8 ++
 18 files changed, 126 insertions(+), 130 deletions(-)
 create mode 100644 consai_ros2/consai_visualizer_msgs/consai_visualizer_msgs/__init__.py
 create mode 100644 consai_ros2/consai_visualizer_msgs/include/consai_visualizer_msgs/manual_conversions.hpp
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/Color.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/Objects.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/Point.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeAnnotation.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeArc.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeCircle.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeLine.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapePoint.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeRectangle.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeRobot.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeText.msg
 delete mode 100644 consai_ros2/consai_visualizer_msgs/msg/ShapeTube.msg
 create mode 100644 consai_ros2/consai_visualizer_msgs/src/manual_conversions.cpp

diff --git a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
index c19fc1821..ee751d527 100644
--- a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
+++ b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
@@ -2,39 +2,115 @@ cmake_minimum_required(VERSION 3.8)
 project(consai_visualizer_msgs)
 
 add_compile_options(-g)
+if(POLICY CMP0148)
+    cmake_policy(SET CMP0148 OLD)  # to accommodate rosidl pipeline
+endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(proto2ros REQUIRED)
 
-set(msg_files
-  "msg/Color.msg"
-  "msg/Objects.msg"
-  "msg/ObjectsArray.msg"
-  "msg/Point.msg"
-  "msg/ShapeAnnotation.msg"
-  "msg/ShapeArc.msg"
-  "msg/ShapeCircle.msg"
-  "msg/ShapeLine.msg"
-  "msg/ShapePoint.msg"
-  "msg/ShapeRectangle.msg"
-  "msg/ShapeRobot.msg"
-  "msg/ShapeText.msg"
-  "msg/ShapeTube.msg"
-)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  ${msg_files}
-  DEPENDENCIES std_msgs
-  # ADD_LINTER_TESTS
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-endif()
+find_package(Protobuf REQUIRED)
+
+## protobufのPythonコード生成
+protobuf_generate(
+        LANGUAGE python
+        OUT_VAR proto_py_sources
+        PROTOS proto/ObjectsArray.proto
+)
+
+# protobufのC++コード生成
+protobuf_generate(
+        LANGUAGE cpp
+        OUT_VAR proto_cpp_sources
+        PROTOS proto/ObjectsArray.proto
+)
+
+add_library(${PROJECT_NAME}_proto SHARED ${proto_cpp_sources})
+target_include_directories(${PROJECT_NAME}_proto PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(${PROJECT_NAME}_proto protobuf::libprotobuf)
+
+## proto -> ROS msg
+proto2ros_generate(
+        ${PROJECT_NAME}_messages_gen
+        PROTOS proto/ObjectsArray.proto
+        #  CONFIG_OVERLAYS config/overlay.yaml
+        INTERFACES_OUT_VAR ros_messages
+        PYTHON_OUT_VAR py_sources
+        CPP_OUT_VAR cpp_sources
+        INCLUDE_OUT_VAR cpp_include_dir
+        APPEND_PYTHONPATH "${CMAKE_CURRENT_BINARY_DIR}/proto"
+        DEPENDS ${proto_py_sources} ${proto_cpp_sources}
+)
+
+# ROS msg -> C++ / Python
+rosidl_generate_interfaces(
+        ${PROJECT_NAME} ${ros_messages}
+        DEPENDENCIES builtin_interfaces proto2ros
+)
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_messages_gen)
 
-ament_export_dependencies(rosidl_default_runtime)
+##　変換ライブラリ
+add_library(${PROJECT_NAME}_conversions SHARED ${cpp_sources} src/manual_conversions.cpp)
+set_target_properties(${PROJECT_NAME}_conversions PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED ON)
+target_compile_options(${PROJECT_NAME}_conversions PUBLIC "$<$<CONFIG:DEBUG>:-O0>")
+target_compile_definitions(${PROJECT_NAME}_conversions
+        PUBLIC
+        "$<$<CONFIG:DEBUG>:DEBUG>"
+        "$<$<OR:$<CONFIG:RELWITHDEBINFO>,$<CONFIG:RELEASE>>:NDEBUG>"
+)
+
+
+target_include_directories(${PROJECT_NAME}_conversions PUBLIC
+        "$<BUILD_INTERFACE:${cpp_include_dir}>"
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/proto>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+rosidl_get_typesupport_target(${PROJECT_NAME}_cpp_msgs ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(${PROJECT_NAME}_conversions
+        ${${PROJECT_NAME}_cpp_msgs} ${PROJECT_NAME}_proto protobuf::libprotobuf)
+ament_target_dependencies(${PROJECT_NAME}_conversions
+        builtin_interfaces proto2ros rclcpp)
+
+rosidl_generated_python_package_add(
+        ${PROJECT_NAME}_additional_modules
+        MODULES ${proto_py_sources} ${py_sources}
+        PACKAGES ${PROJECT_NAME}
+        DESTINATION ${PROJECT_NAME}
+)
+
+# Install generated C++ .pb.h and conversion headers
+set(cpp_headers ${cpp_sources} ${proto_cpp_sources})
+list(FILTER cpp_headers INCLUDE REGEX ".*\.(hpp|h)$")
+install(
+        FILES ${cpp_headers}
+        DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME}/
+)
+
+install(
+        FILES ${CMAKE_CURRENT_BINARY_DIR}/proto/ObjectsArray.pb.h
+        DESTINATION include/${PROJECT_NAME}/
+)
+
+# Install C++ Protobuf messages and conversion libraries
+install(
+        TARGETS
+        ${PROJECT_NAME}_proto
+        ${PROJECT_NAME}_conversions
+        #  EXPORT ${PROJECT_NAME}
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+ament_export_dependencies(builtin_interfaces proto2ros rclcpp)
+#ament_export_targets(${PROJECT_NAME})
 
 ament_package()
diff --git a/consai_ros2/consai_visualizer_msgs/consai_visualizer_msgs/__init__.py b/consai_ros2/consai_visualizer_msgs/consai_visualizer_msgs/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/consai_ros2/consai_visualizer_msgs/include/consai_visualizer_msgs/manual_conversions.hpp b/consai_ros2/consai_visualizer_msgs/include/consai_visualizer_msgs/manual_conversions.hpp
new file mode 100644
index 000000000..a25f6043b
--- /dev/null
+++ b/consai_ros2/consai_visualizer_msgs/include/consai_visualizer_msgs/manual_conversions.hpp
@@ -0,0 +1,10 @@
+// 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.
+
+#ifndef CONSAI_VISUALIZER_MSGS__MANUAL_CONVERSIONS_HPP_
+#define CONSAI_VISUALIZER_MSGS__MANUAL_CONVERSIONS_HPP_
+
+#endif  // CONSAI_VISUALIZER_MSGS__MANUAL_CONVERSIONS_HPP_
diff --git a/consai_ros2/consai_visualizer_msgs/msg/Color.msg b/consai_ros2/consai_visualizer_msgs/msg/Color.msg
deleted file mode 100644
index ee5e5a76c..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/Color.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-
-# Support color names:https://www.w3.org/TR/SVG11/types.html#ColorKeywords
-string name ""
-# RGBA : 0.0 ~ 1.0
-float32 red 0.0
-float32 green 0.0
-float32 blue 0.0
-float32 alpha 1.0
diff --git a/consai_ros2/consai_visualizer_msgs/msg/Objects.msg b/consai_ros2/consai_visualizer_msgs/msg/Objects.msg
deleted file mode 100644
index 540fb0bbf..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/Objects.msg
+++ /dev/null
@@ -1,17 +0,0 @@
-
-string layer "default"
-string sub_layer "default"
-uint32 z_order 0  # 数字が大きいほど手前に表示される
-
-# ウィンドウ領域に描画するオブジェクト
-ShapeAnnotation[] annotations
-
-# フィールド領域に描画するオブジェクト
-ShapePoint[] points
-ShapeLine[] lines
-ShapeArc[] arcs
-ShapeRectangle[] rects
-ShapeCircle[] circles
-ShapeTube[] tubes
-ShapeText[] texts
-ShapeRobot[] robots
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg b/consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg
deleted file mode 100644
index f7830ffc5..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ObjectsArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-Objects[] objects
diff --git a/consai_ros2/consai_visualizer_msgs/msg/Point.msg b/consai_ros2/consai_visualizer_msgs/msg/Point.msg
deleted file mode 100644
index cbd109144..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/Point.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-
-# unit: meters
-float32 x
-float32 y
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeAnnotation.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeAnnotation.msg
deleted file mode 100644
index 4d77dde07..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeAnnotation.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-
-# 0.0 ~ 1.0
-float32 normalized_x
-float32 normalized_y
-float32 normalized_width
-float32 normalized_height
-string text
-Color color
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeArc.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeArc.msg
deleted file mode 100644
index 38cdbc6eb..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeArc.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-
-Point center
-float32 radius
-float32 start_angle
-float32 end_angle
-uint32 size
-Color color
-string caption
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeCircle.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeCircle.msg
deleted file mode 100644
index 09501347a..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeCircle.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-
-Point center
-float32 radius
-Color line_color
-Color fill_color
-uint32 line_size
-string caption
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeLine.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeLine.msg
deleted file mode 100644
index caa99afe1..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeLine.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-
-Point p1
-Point p2
-uint32 size
-Color color
-string caption
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapePoint.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapePoint.msg
deleted file mode 100644
index 0a9d4295a..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapePoint.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-
-# unit: meters
-float32 x
-float32 y
-uint32 size
-Color color
-string caption
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeRectangle.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeRectangle.msg
deleted file mode 100644
index e4ef2d34e..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeRectangle.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-
-Point center
-float32 width
-float32 height
-uint32 line_size
-Color line_color
-Color fill_color
-string caption
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeRobot.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeRobot.msg
deleted file mode 100644
index 6938b187f..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeRobot.msg
+++ /dev/null
@@ -1,14 +0,0 @@
-
-uint32 COLOR_TYPE_DEFAULT = 0
-uint32 COLOR_TYPE_REAL = 1
-
-uint32 color_type 0
-float32 radius 0.09
-Color line_color
-Color fill_color
-uint32 line_size
-uint32 id
-float32 x
-float32 y
-float32 theta
-string caption
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeText.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeText.msg
deleted file mode 100644
index 6cda76476..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeText.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-
-# unit: meters
-float32 x
-float32 y
-string text
-uint32 size
-Color color
diff --git a/consai_ros2/consai_visualizer_msgs/msg/ShapeTube.msg b/consai_ros2/consai_visualizer_msgs/msg/ShapeTube.msg
deleted file mode 100644
index 60375d51a..000000000
--- a/consai_ros2/consai_visualizer_msgs/msg/ShapeTube.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-
-Point p1
-Point p2
-float32 radius
-Color line_color
-Color fill_color
-uint32 line_size
-string caption
diff --git a/consai_ros2/consai_visualizer_msgs/package.xml b/consai_ros2/consai_visualizer_msgs/package.xml
index 7d15b0b81..9059f6eb8 100644
--- a/consai_ros2/consai_visualizer_msgs/package.xml
+++ b/consai_ros2/consai_visualizer_msgs/package.xml
@@ -11,6 +11,11 @@
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>action_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>proto2ros</depend>
+  <depend>protobuf</depend>
+  <depend>protobuf-dev</depend>
+  <depend>rclcpp</depend>
   <depend>std_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
diff --git a/consai_ros2/consai_visualizer_msgs/src/manual_conversions.cpp b/consai_ros2/consai_visualizer_msgs/src/manual_conversions.cpp
new file mode 100644
index 000000000..b62e885bc
--- /dev/null
+++ b/consai_ros2/consai_visualizer_msgs/src/manual_conversions.cpp
@@ -0,0 +1,8 @@
+// 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.
+
+// NOTE: 名前もmanual_conversions.cppで固定
+#include <consai_visualizer_msgs/manual_conversions.hpp>

From a1f89812ecf87ce832893b6298b737a59c2bb1d8 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 00:29:13 +0900
Subject: [PATCH 03/14] =?UTF-8?q?=E3=83=93=E3=83=AB=E3=83=89=E3=82=A8?=
 =?UTF-8?q?=E3=83=A9=E3=83=BC=E4=BF=AE=E6=AD=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../crane_msg_wrappers/consai_visualizer_wrapper.hpp      | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp
index 4f0e21a64..5128e3f38 100644
--- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp
+++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp
@@ -464,19 +464,19 @@ struct ShapeRobotBuilder : public FillShapeColorBuilder<consai_visualizer_msgs::
 
   ShapeRobotBuilder & color_type(bool color_type)
   {
-    robot.color_type = color_type;
+    robot.color_type.value = color_type;
     return *this;
   }
 
   ShapeRobotBuilder & color_type_default()
   {
-    robot.color_type = 0;
+    robot.color_type.value = 0;
     return *this;
   }
 
   ShapeRobotBuilder & color_type_real()
   {
-    robot.color_type = 1;
+    robot.color_type.value = 1;
     return *this;
   }
 
@@ -866,7 +866,7 @@ struct ConsaiVisualizerBuffer
       robot.line_size = line_size;
       robot.caption = caption;
       robot.id = id;
-      robot.color_type = color_type;
+      robot.color_type.value = color_type;
       message_buffer.robots.push_back(robot);
     }
 

From 7544d6a8cbb23cefbce703fd9bef7b1bd7006199 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 01:48:15 +0900
Subject: [PATCH 04/14] =?UTF-8?q?=E3=82=A8=E3=82=AF=E3=82=B9=E3=83=9D?=
 =?UTF-8?q?=E3=83=BC=E3=83=88=E5=BF=98=E3=82=8C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 consai_ros2/consai_visualizer_msgs/CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
index ee751d527..60b8ac5b2 100644
--- a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
+++ b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
@@ -105,12 +105,12 @@ install(
         TARGETS
         ${PROJECT_NAME}_proto
         ${PROJECT_NAME}_conversions
-        #  EXPORT ${PROJECT_NAME}
+        EXPORT ${PROJECT_NAME}
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
         RUNTIME DESTINATION bin
 )
 ament_export_dependencies(builtin_interfaces proto2ros rclcpp)
-#ament_export_targets(${PROJECT_NAME})
+ament_export_targets(${PROJECT_NAME})
 
 ament_package()

From dc627e000e8df0ef67d8f5d0763ad663167407c9 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 03:47:06 +0900
Subject: [PATCH 05/14] =?UTF-8?q?=E5=A4=B1=E3=82=8F=E3=82=8C=E3=81=9F?=
 =?UTF-8?q?=E3=83=87=E3=83=95=E3=82=A9=E3=83=AB=E3=83=88=E5=80=A4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../src/visualization_data_handler.cpp                | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp
index 81ea978c8..9dc4cc1e6 100644
--- a/crane_world_model_publisher/src/visualization_data_handler.cpp
+++ b/crane_world_model_publisher/src/visualization_data_handler.cpp
@@ -58,6 +58,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo
     VisLine line;
 
     line.color.name = "white";
+    line.color.alpha = 1.0;
     line.size = 2;
     // 単位を[m]に変換
     line.p1.x = field_line.p1().x() * 0.001;
@@ -73,6 +74,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo
     VisArc arc;
 
     arc.color.name = "white";
+    arc.color.alpha = 1.0;
     arc.size = 2;
     // 単位を[m]に変換
     arc.center.x = field_arc.center().x() * 0.001;
@@ -89,6 +91,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo
   // Ref: https://robocup-ssl.github.io/ssl-rules/sslrules.html#_penalty_mark
   VisPoint point;
   point.color.name = "white";
+  point.color.alpha = 1.0;
   point.size = 6;
   point.x = -geometry_data.field().field_length() * 0.001 / 2.0 + 8.0;
   point.y = 0.0;
@@ -102,6 +105,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo
   // フィールドの枠
   VisRect rect;
   rect.line_color.name = "black";
+  rect.line_color.alpha = 1.0;
   rect.fill_color.alpha = 0.0;
   rect.line_size = 3;
   rect.center.x = 0.0;
@@ -144,6 +148,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_
     // ボールは小さいのでボールの周りを大きな円で囲う
     vis_ball.line_color.name = "crimson";
     vis_ball.fill_color.alpha = 0.0;
+    vis_ball.line_color.alpha = 1.0;
     vis_ball.line_size = 2;
     vis_ball.radius = 0.8;
     vis_ball.caption = "ball is here";
@@ -168,6 +173,9 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_
   VisRobot vis_robot;
   vis_robot.line_color.name = "black";
   vis_robot.line_size = 1;
+  vis_robot.fill_color.alpha = 1.0;
+  vis_robot.line_color.alpha = 1.0;
+  vis_robot.radius = 0.09;
   for (const auto & robot : tracked_frame.robots()) {
     if (not robot.has_visibility() || robot.visibility() < 0.5) {
       continue;
@@ -367,6 +375,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg)
   VisAnnotation vis_annotation;
   vis_annotation.text = parse_stage(msg->stage);
   vis_annotation.color.name = "white";
+  vis_annotation.color.alpha = 1.0;
   vis_annotation.normalized_x = STAGE_COMMAND_X;
   vis_annotation.normalized_y = 0.0;
   vis_annotation.normalized_width = STAGE_COMMAND_WIDTH;
@@ -523,7 +532,9 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg)
       vis_circle.center.y = msg->designated_position.front().y;
       vis_circle.radius = 0.15;
       vis_circle.line_color.name = "aquamarine";
+      vis_circle.line_color.alpha = 1.0;
       vis_circle.fill_color.name = "aquamarine";
+      vis_circle.fill_color.alpha = 1.0;
       vis_circle.line_size = 1;
       vis_circle.caption = "placement pos";
       vis_objects.circles.push_back(vis_circle);

From 8057de7d038ab8bbf4f29b40d05f16765558f1a1 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 03:47:45 +0900
Subject: [PATCH 06/14] =?UTF-8?q?QoS=E3=82=92=E5=90=88=E3=82=8F=E3=81=9B?=
 =?UTF-8?q?=E3=82=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../include/crane_msg_wrappers/consai_visualizer_wrapper.hpp    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp
index 5128e3f38..d395d69ff 100644
--- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp
+++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp
@@ -567,7 +567,7 @@ struct ConsaiVisualizerBuffer
   template <typename Node>
   ConsaiVisualizerBuffer(Node & node, const std::string topic)
   {
-    publisher = node.template create_publisher<ObjectsArray>(topic, 10);
+    publisher = node.template create_publisher<ObjectsArray>(topic, rclcpp::SensorDataQoS());
   }
 
   template <typename Node>

From b552f1fa71706b815a41848ff40e334566b175e7 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 19:09:27 +0900
Subject: [PATCH 07/14] =?UTF-8?q?proto=E3=81=AE=E8=BF=BD=E5=8A=A0=E5=BF=98?=
 =?UTF-8?q?=E3=82=8C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../proto/ObjectsArray.proto                  | 142 ++++++++++++++++++
 1 file changed, 142 insertions(+)
 create mode 100644 consai_ros2/consai_visualizer_msgs/proto/ObjectsArray.proto

diff --git a/consai_ros2/consai_visualizer_msgs/proto/ObjectsArray.proto b/consai_ros2/consai_visualizer_msgs/proto/ObjectsArray.proto
new file mode 100644
index 000000000..ab48b5d87
--- /dev/null
+++ b/consai_ros2/consai_visualizer_msgs/proto/ObjectsArray.proto
@@ -0,0 +1,142 @@
+syntax = "proto3";
+
+package visualizer;
+
+// Color.msg
+message Color {
+  string name = 1;
+  float red = 2;
+  float green = 3;
+  float blue = 4;
+  float alpha = 5;
+}
+
+// Point.msg
+message Point {
+  float x = 1;  // unit: meters
+  float y = 2;  // unit: meters
+}
+
+// ShapeAnnotation.msg
+message ShapeAnnotation {
+  float normalized_x = 1;
+  float normalized_y = 2;
+  float normalized_width = 3;
+  float normalized_height = 4;
+  string text = 5;
+  Color color = 6;
+}
+
+// ShapeArc.msg
+message ShapeArc {
+  Point center = 1;
+  float radius = 2;
+  float start_angle = 3;
+  float end_angle = 4;
+  uint32 size = 5;
+  Color color = 6;
+  string caption = 7;
+}
+
+// ShapeCircle.msg
+message ShapeCircle {
+  Point center = 1;
+  float radius = 2;
+  Color line_color = 3;
+  Color fill_color = 4;
+  uint32 line_size = 5;
+  string caption = 6;
+}
+
+// ShapeLine.msg
+message ShapeLine {
+  Point p1 = 1;
+  Point p2 = 2;
+  uint32 size = 3;
+  Color color = 4;
+  string caption = 5;
+}
+
+// ShapePoint.msg
+message ShapePoint {
+  float x = 1;  // unit: meters
+  float y = 2;  // unit: meters
+  uint32 size = 3;
+  Color color = 4;
+  string caption = 5;
+}
+
+// ShapeRectangle.msg
+message ShapeRectangle {
+  Point center = 1;
+  float width = 2;
+  float height = 3;
+  uint32 line_size = 4;
+  Color line_color = 5;
+  Color fill_color = 6;
+  string caption = 7;
+}
+
+// ShapeRobot.msg
+message ShapeRobot {
+  enum ColorType {
+    COLOR_TYPE_DEFAULT = 0;
+    COLOR_TYPE_REAL = 1;
+  }
+
+  ColorType color_type = 1;
+  float radius = 2;
+  Color line_color = 3;
+  Color fill_color = 4;
+  uint32 line_size = 5;
+  uint32 id = 6;
+  float x = 7;
+  float y = 8;
+  float theta = 9;
+  string caption = 10;
+}
+
+// ShapeText.msg
+message ShapeText {
+  float x = 1;  // unit: meters
+  float y = 2;  // unit: meters
+  string text = 3;
+  uint32 size = 4;
+  Color color = 5;
+}
+
+// ShapeTube.msg
+message ShapeTube {
+  Point p1 = 1;
+  Point p2 = 2;
+  float radius = 3;
+  Color line_color = 4;
+  Color fill_color = 5;
+  uint32 line_size = 6;
+  string caption = 7;
+}
+
+// Objects.msg
+message Objects {
+  string layer = 1;
+  string sub_layer = 2;
+  uint32 z_order = 3;  // Higher numbers are displayed in front
+
+  // Objects drawn in the window area
+  repeated ShapeAnnotation annotations = 4;
+
+  // Objects drawn in the field area
+  repeated ShapePoint points = 5;
+  repeated ShapeLine lines = 6;
+  repeated ShapeArc arcs = 7;
+  repeated ShapeRectangle rects = 8;
+  repeated ShapeCircle circles = 9;
+  repeated ShapeTube tubes = 10;
+  repeated ShapeText texts = 11;
+  repeated ShapeRobot robots = 12;
+}
+
+// ObjectsArray.msg
+message ObjectsArray {
+  repeated Objects objects = 1;
+}

From 281a1cd3ab96791dc317b0d21a8e179a86528de8 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 21:00:11 +0900
Subject: [PATCH 08/14] =?UTF-8?q?=E8=BE=9E=E6=9B=B8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .github/workflows/custom_dict.json | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/custom_dict.json b/.github/workflows/custom_dict.json
index 2b4c7acc8..96f997979 100644
--- a/.github/workflows/custom_dict.json
+++ b/.github/workflows/custom_dict.json
@@ -120,6 +120,10 @@
     "CHECKVERSION",
     "BLAUE",
     "Hackentrick",
-    "3rdparty"
+    "3rdparty",
+    "PROTOS",
+    "libprotobuf",
+    "RELWITHDEBINFO",
+    "NODEBUG"
   ]
 }

From 25075fab7b8d3b0ec728e25cf06f5b2fd01ad3f5 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 21:00:26 +0900
Subject: [PATCH 09/14] =?UTF-8?q?CMakeLists.txt=E3=82=AF=E3=83=AA=E3=83=BC?=
 =?UTF-8?q?=E3=83=B3=E3=82=A2=E3=83=83=E3=83=97?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 consai_ros2/consai_visualizer_msgs/CMakeLists.txt | 11 -----------
 1 file changed, 11 deletions(-)

diff --git a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
index 60b8ac5b2..d42fe120b 100644
--- a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
+++ b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
@@ -2,9 +2,6 @@ cmake_minimum_required(VERSION 3.8)
 project(consai_visualizer_msgs)
 
 add_compile_options(-g)
-if(POLICY CMP0148)
-    cmake_policy(SET CMP0148 OLD)  # to accommodate rosidl pipeline
-endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -59,14 +56,6 @@ add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_messages_gen)
 
 ##　変換ライブラリ
 add_library(${PROJECT_NAME}_conversions SHARED ${cpp_sources} src/manual_conversions.cpp)
-set_target_properties(${PROJECT_NAME}_conversions PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED ON)
-target_compile_options(${PROJECT_NAME}_conversions PUBLIC "$<$<CONFIG:DEBUG>:-O0>")
-target_compile_definitions(${PROJECT_NAME}_conversions
-        PUBLIC
-        "$<$<CONFIG:DEBUG>:DEBUG>"
-        "$<$<OR:$<CONFIG:RELWITHDEBINFO>,$<CONFIG:RELEASE>>:NDEBUG>"
-)
-
 
 target_include_directories(${PROJECT_NAME}_conversions PUBLIC
         "$<BUILD_INTERFACE:${cpp_include_dir}>"

From 91de1428578159d56635053e865fe32af9487fa2 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 21:07:43 +0900
Subject: [PATCH 10/14] =?UTF-8?q?=E7=94=9F=E6=88=90=E3=82=B3=E3=83=BC?=
 =?UTF-8?q?=E3=83=89=E3=81=BE=E3=81=A7=E3=83=86=E3=82=B9=E3=83=88=E3=81=95?=
 =?UTF-8?q?=E3=82=8C=E3=81=A6=E3=81=AF=E6=95=B5=E3=82=8F=E3=81=AA=E3=81=84?=
 =?UTF-8?q?=E3=81=AE=E3=81=A7OFF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 consai_ros2/consai_visualizer_msgs/package.xml | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/consai_ros2/consai_visualizer_msgs/package.xml b/consai_ros2/consai_visualizer_msgs/package.xml
index 9059f6eb8..d82647d5f 100644
--- a/consai_ros2/consai_visualizer_msgs/package.xml
+++ b/consai_ros2/consai_visualizer_msgs/package.xml
@@ -20,9 +20,6 @@
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
   <member_of_group>rosidl_interface_packages</member_of_group>
 
   <export>

From c14ef416d9bdeaad0917cc111647bdbf1f3d931a Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Mon, 23 Dec 2024 21:11:55 +0900
Subject: [PATCH 11/14] pre-commit

---
 .vscode/launch.json | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/.vscode/launch.json b/.vscode/launch.json
index ddf311797..3e6587c86 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -1,7 +1,4 @@
 {
-  // Use IntelliSense to learn about possible attributes.
-  // Hover to view descriptions of existing attributes.
-  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
       {

From d26c49293a3c52ed4844d792e0c7540f4db7e5a3 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Tue, 24 Dec 2024 01:24:12 +0900
Subject: [PATCH 12/14] =?UTF-8?q?consai=5Fvisualizer=5Fmsgs=E3=83=91?=
 =?UTF-8?q?=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=E3=81=AE=E6=95=B4=E7=90=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../consai_visualizer_msgs/CMakeLists.txt     | 28 ++++---------------
 .../consai_visualizer_msgs/package.xml        |  2 +-
 .../proto/CMakeLists.txt                      | 26 +++++++++++++++++
 3 files changed, 32 insertions(+), 24 deletions(-)
 create mode 100644 consai_ros2/consai_visualizer_msgs/proto/CMakeLists.txt

diff --git a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
index d42fe120b..7f845d29c 100644
--- a/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
+++ b/consai_ros2/consai_visualizer_msgs/CMakeLists.txt
@@ -13,41 +13,23 @@ find_package(proto2ros REQUIRED)
 
 find_package(Protobuf REQUIRED)
 
-## protobufのPythonコード生成
-protobuf_generate(
-        LANGUAGE python
-        OUT_VAR proto_py_sources
-        PROTOS proto/ObjectsArray.proto
-)
-
-# protobufのC++コード生成
-protobuf_generate(
-        LANGUAGE cpp
-        OUT_VAR proto_cpp_sources
-        PROTOS proto/ObjectsArray.proto
-)
-
-add_library(${PROJECT_NAME}_proto SHARED ${proto_cpp_sources})
-target_include_directories(${PROJECT_NAME}_proto PUBLIC
-        "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>"
-        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-)
-target_link_libraries(${PROJECT_NAME}_proto protobuf::libprotobuf)
+add_subdirectory(proto)
 
-## proto -> ROS msg
+### proto -> ROS msg
 proto2ros_generate(
         ${PROJECT_NAME}_messages_gen
         PROTOS proto/ObjectsArray.proto
+        IMPORT_DIRS proto
         #  CONFIG_OVERLAYS config/overlay.yaml
         INTERFACES_OUT_VAR ros_messages
         PYTHON_OUT_VAR py_sources
         CPP_OUT_VAR cpp_sources
         INCLUDE_OUT_VAR cpp_include_dir
         APPEND_PYTHONPATH "${CMAKE_CURRENT_BINARY_DIR}/proto"
-        DEPENDS ${proto_py_sources} ${proto_cpp_sources}
+        DEPENDS ${PROJECT_NAME}_proto_gen
 )
 
-# ROS msg -> C++ / Python
+## ROS msg -> C++ / Python
 rosidl_generate_interfaces(
         ${PROJECT_NAME} ${ros_messages}
         DEPENDENCIES builtin_interfaces proto2ros
diff --git a/consai_ros2/consai_visualizer_msgs/package.xml b/consai_ros2/consai_visualizer_msgs/package.xml
index d82647d5f..43f49508f 100644
--- a/consai_ros2/consai_visualizer_msgs/package.xml
+++ b/consai_ros2/consai_visualizer_msgs/package.xml
@@ -10,7 +10,6 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
-  <depend>action_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>proto2ros</depend>
   <depend>protobuf</depend>
@@ -18,6 +17,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
 
+  <exec_depend>python3-protobuf</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
diff --git a/consai_ros2/consai_visualizer_msgs/proto/CMakeLists.txt b/consai_ros2/consai_visualizer_msgs/proto/CMakeLists.txt
new file mode 100644
index 000000000..a6bb5840e
--- /dev/null
+++ b/consai_ros2/consai_visualizer_msgs/proto/CMakeLists.txt
@@ -0,0 +1,26 @@
+find_package(Protobuf REQUIRED)
+
+protobuf_generate(
+        LANGUAGE python
+        OUT_VAR proto_py_sources
+        PROTOS ObjectsArray.proto
+)
+
+protobuf_generate(
+        LANGUAGE cpp
+        OUT_VAR proto_cpp_sources
+        PROTOS ObjectsArray.proto
+)
+
+add_library(${PROJECT_NAME}_proto SHARED ${proto_cpp_sources} )
+#target_include_directories(${PROJECT_NAME}_proto PUBLIC "${CMAKE_CURRENT_BINARY_DIR}")
+target_compile_options(${PROJECT_NAME}_proto PUBLIC -Wno-deprecated-declarations)
+target_link_libraries(${PROJECT_NAME}_proto protobuf::libprotobuf)
+
+add_custom_target(
+        ${PROJECT_NAME}_proto_gen ALL DEPENDS
+        ${proto_py_sources}
+        ${proto_cpp_sources}
+)
+
+set(PROTO_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}" PARENT_SCOPE)

From 34547b3b1bfd4027c3cc815470d282ce2ae16e7e Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Tue, 24 Dec 2024 01:29:05 +0900
Subject: [PATCH 13/14] =?UTF-8?q?crane=5Fsender=E3=83=91=E3=83=83=E3=82=B1?=
 =?UTF-8?q?=E3=83=BC=E3=82=B8=E3=81=AE=E6=95=B4=E7=90=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 crane_sender/CMakeLists.txt          |  2 +-
 crane_sender/config/grsim.yaml       |  4 ---
 crane_sender/launch/sender.launch.py | 42 ----------------------------
 3 files changed, 1 insertion(+), 47 deletions(-)
 delete mode 100755 crane_sender/config/grsim.yaml
 delete mode 100755 crane_sender/launch/sender.launch.py

diff --git a/crane_sender/CMakeLists.txt b/crane_sender/CMakeLists.txt
index 5cc17aaf7..fd9a63dc2 100755
--- a/crane_sender/CMakeLists.txt
+++ b/crane_sender/CMakeLists.txt
@@ -30,4 +30,4 @@ if (BUILD_TESTING)
   target_link_libraries(test_robot_packet sim_sender_component)
 endif ()
 
-ament_auto_package(INSTALL_TO_SHARE launch config)
+ament_auto_package()
diff --git a/crane_sender/config/grsim.yaml b/crane_sender/config/grsim.yaml
deleted file mode 100755
index fafcea0ee..000000000
--- a/crane_sender/config/grsim.yaml
+++ /dev/null
@@ -1,4 +0,0 @@
-sim_sender:
-  ros__parameters:
-    grsim_addr: 127.0.0.1
-    grsim_port: 20011
diff --git a/crane_sender/launch/sender.launch.py b/crane_sender/launch/sender.launch.py
deleted file mode 100755
index bd552e3cc..000000000
--- a/crane_sender/launch/sender.launch.py
+++ /dev/null
@@ -1,42 +0,0 @@
-# Copyright (c) 2019 SSL-Roots
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
-# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    start_sender_cmd = Node(
-        package="crane_sender",
-        node_executable="sim_sender",
-        output="screen",
-        parameters=[
-            os.path.join(get_package_share_directory("crane_sender"), "config", "grsim.yaml")
-        ],
-    )
-
-    ld = LaunchDescription()
-
-    ld.add_action(start_sender_cmd)
-
-    return ld

From bd0cba0bfc86608cfb9878e60bb752f1c929f7d3 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Tue, 24 Dec 2024 01:40:19 +0900
Subject: [PATCH 14/14] =?UTF-8?q?crane=5Fvisualization=5Fbridge=E3=83=91?=
 =?UTF-8?q?=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=E3=82=92=E8=BF=BD=E5=8A=A0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../crane_visualization_bridge/.clang-format  |  17 ++
 .../crane_visualization_bridge/CMakeLists.txt |  24 ++
 .../crane_visualization_bridge/package.xml    |  24 ++
 .../src/crane_visualization_bridge_node.cpp   | 242 ++++++++++++++++++
 4 files changed, 307 insertions(+)
 create mode 100755 utility/crane_visualization_bridge/.clang-format
 create mode 100755 utility/crane_visualization_bridge/CMakeLists.txt
 create mode 100755 utility/crane_visualization_bridge/package.xml
 create mode 100644 utility/crane_visualization_bridge/src/crane_visualization_bridge_node.cpp

diff --git a/utility/crane_visualization_bridge/.clang-format b/utility/crane_visualization_bridge/.clang-format
new file mode 100755
index 000000000..1db9fb9f5
--- /dev/null
+++ b/utility/crane_visualization_bridge/.clang-format
@@ -0,0 +1,17 @@
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false
diff --git a/utility/crane_visualization_bridge/CMakeLists.txt b/utility/crane_visualization_bridge/CMakeLists.txt
new file mode 100755
index 000000000..b0c7d158f
--- /dev/null
+++ b/utility/crane_visualization_bridge/CMakeLists.txt
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.5)
+project(crane_visualization_bridge)
+
+# Default to C++20
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -g)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()
diff --git a/utility/crane_visualization_bridge/package.xml b/utility/crane_visualization_bridge/package.xml
new file mode 100755
index 000000000..2378e4a88
--- /dev/null
+++ b/utility/crane_visualization_bridge/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>crane_visualization_bridge</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ibis.ssl.team@gmail.com">ibis ssl</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>consai_visualizer_msgs</depend>
+  <depend>crane_msgs</depend>
+  <depend>proto2ros</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>crane_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/utility/crane_visualization_bridge/src/crane_visualization_bridge_node.cpp b/utility/crane_visualization_bridge/src/crane_visualization_bridge_node.cpp
new file mode 100644
index 000000000..837163199
--- /dev/null
+++ b/utility/crane_visualization_bridge/src/crane_visualization_bridge_node.cpp
@@ -0,0 +1,242 @@
+// 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 <boost/asio.hpp>
+#include <boost/beast.hpp>
+#include <boost/beast/websocket.hpp>
+#include <chrono>
+#include <consai_visualizer_msgs/conversions.hpp>
+#include <consai_visualizer_msgs/msg/objects_array.hpp>
+#include <memory>
+#include <proto2ros/conversions.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <unordered_map>
+
+class WebSocketClient
+{
+public:
+  explicit WebSocketClient(const std::string & uri) : uri_(uri), stop_flag_(false)
+  {
+    parseUri(uri);
+    startConnection();
+    sender_thread_ = std::thread([this]() { processQueue(); });
+  }
+
+  ~WebSocketClient()
+  {
+    stop_flag_ = true;
+    queue_cond_var_.notify_all();
+
+    if (worker_thread_.joinable()) {
+      worker_thread_.join();
+    }
+    if (sender_thread_.joinable()) {
+      sender_thread_.join();
+    }
+  }
+
+  void send(const std::string & message)
+  {
+    std::lock_guard<std::mutex> lock(queue_mutex_);
+    message_queue_.push(message);
+    queue_cond_var_.notify_one();
+  }
+
+  bool isConnected() const
+  {
+    std::lock_guard<std::mutex> lock(connection_mutex_);
+    return is_connected_;
+  }
+
+private:
+  void parseUri(const std::string & uri)
+  {
+    auto pos = uri.find("://");
+    if (pos == std::string::npos) {
+      throw std::invalid_argument("Invalid URI format");
+    }
+    protocol_ = uri.substr(0, pos);
+    auto host_and_port = uri.substr(pos + 3);
+    auto port_pos = host_and_port.find(':');
+    if (port_pos == std::string::npos) {
+      host_ = host_and_port;
+      port_ = "80";  // Default port
+    } else {
+      host_ = host_and_port.substr(0, port_pos);
+      port_ = host_and_port.substr(port_pos + 1);
+    }
+    std::cout << "Parsed URI: " << protocol_ << "://" << host_ << ":" << port_ << std::endl;
+  }
+
+  void createNewConnection()
+  {
+    std::cout << "Creating new WebSocket connection to " << host_ << ":" << port_ << std::endl;
+
+    auto ioc = std::make_shared<boost::asio::io_context>();
+    auto resolver = boost::asio::ip::tcp::resolver(*ioc);
+    auto ws = std::make_unique<boost::beast::websocket::stream<boost::asio::ip::tcp::socket>>(*ioc);
+
+    try {
+      // Set these options before the handshake
+      ws->set_option(boost::beast::websocket::stream_base::decorator(
+        [](boost::beast::websocket::request_type & req) {
+          req.set(
+            boost::beast::http::field::user_agent,
+            std::string(BOOST_BEAST_VERSION_STRING) + " websocket-client-coro");
+        }));
+
+      std::cout << "Resolving host..." << std::endl;
+      auto const results = resolver.resolve(host_, port_);
+
+      std::cout << "Connecting to endpoint..." << std::endl;
+      boost::asio::connect(ws->next_layer(), results.begin(), results.end());
+
+      std::cout << "Performing WebSocket handshake..." << std::endl;
+      // Enable binary data
+      ws->binary(true);
+      // Disable compression
+      ws->set_option(
+        boost::beast::websocket::stream_base::timeout::suggested(boost::beast::role_type::client));
+
+      ws->handshake(host_, "/");
+
+      std::cout << "WebSocket connection established successfully" << std::endl;
+
+      {
+        std::lock_guard<std::mutex> lock(connection_mutex_);
+        io_context_ = std::move(ioc);
+        socket_ = std::move(ws);
+        is_connected_ = true;
+        connection_error_.clear();
+      }
+    } catch (const std::exception & e) {
+      std::cout << "Connection failed with error: " << e.what() << std::endl;
+      std::lock_guard<std::mutex> lock(connection_mutex_);
+      is_connected_ = false;
+      connection_error_ = e.what();
+      throw;
+    }
+  }
+
+  void startConnection()
+  {
+    worker_thread_ = std::thread([this]() {
+      while (!stop_flag_) {
+        try {
+          createNewConnection();
+          io_context_->run();
+          break;  // 正常に接続された場合はループを抜ける
+        } catch (const std::exception & e) {
+          std::cout << "Connection failed: " << e.what() << std::endl;
+          // 接続に失敗した場合、3秒待って再試行
+          std::this_thread::sleep_for(std::chrono::seconds(3));
+        }
+      }
+    });
+  }
+
+  void processQueue()
+  {
+    while (!stop_flag_) {
+      std::string message;
+      {
+        std::unique_lock<std::mutex> lock(queue_mutex_);
+        queue_cond_var_.wait(lock, [this]() { return stop_flag_ || !message_queue_.empty(); });
+
+        if (stop_flag_) break;
+
+        message = std::move(message_queue_.front());
+        message_queue_.pop();
+        std::cout << "Processing message of size: " << message.size() << " bytes" << std::endl;
+      }
+
+      bool sent = false;
+      while (!stop_flag_ && !sent) {
+        std::lock_guard<std::mutex> lock(connection_mutex_);
+        if (is_connected_ && socket_) {
+          try {
+            std::cout << "Attempting to send message..." << std::endl;
+            socket_->write(boost::asio::buffer(message));
+            std::cout << "Message sent successfully" << std::endl;
+            sent = true;
+          } catch (const std::exception & e) {
+            std::cout << "Send failed with error: " << e.what() << std::endl;
+            connection_error_ = e.what();
+            is_connected_ = false;
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+            try {
+              std::cout << "Attempting to reconnect..." << std::endl;
+              createNewConnection();
+            } catch (const std::exception & e) {
+              std::cout << "Reconnection failed: " << e.what() << std::endl;
+            }
+          }
+        } else {
+          std::cout << "Not connected, waiting..." << std::endl;
+          std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+      }
+    }
+  }
+
+  std::string uri_;
+  std::string host_;
+  std::string port_;
+  std::string protocol_;
+
+  std::shared_ptr<boost::asio::io_context> io_context_;
+  std::unique_ptr<boost::beast::websocket::stream<boost::asio::ip::tcp::socket>> socket_;
+
+  bool is_connected_ = false;
+  bool stop_flag_ = false;
+  std::string connection_error_;
+
+  mutable std::mutex connection_mutex_;
+
+  std::thread worker_thread_;
+  std::thread sender_thread_;
+
+  std::queue<std::string> message_queue_;
+  std::mutex queue_mutex_;
+  std::condition_variable queue_cond_var_;
+};
+
+class Bridge : public rclcpp::Node
+{
+public:
+  Bridge() : Node("bridge")
+  {
+    std::string websocket_url =
+      this->declare_parameter<std::string>("websocket_url", "ws://localhost:9000");
+    websocket_client = std::make_shared<WebSocketClient>(websocket_url);
+
+    sub_objects_array = create_subscription<consai_visualizer_msgs::msg::ObjectsArray>(
+      "/visualizer_objects", rclcpp::SensorDataQoS(),
+      [&](const consai_visualizer_msgs::msg::ObjectsArray & ros_msg) {
+        // std::cout << "Received ObjectsArray: " << static_cast<int>(ros_msg.objects.size())
+        //           << std::endl;
+        visualizer::ObjectsArray proto_msg;
+        consai_visualizer_msgs::conversions::Convert(ros_msg, &proto_msg);
+        std::string buffer;
+        proto_msg.SerializeToString(&buffer);
+        websocket_client->send(buffer);
+      });
+  }
+
+private:
+  std::shared_ptr<WebSocketClient> websocket_client;
+  rclcpp::Subscription<consai_visualizer_msgs::msg::ObjectsArray>::SharedPtr sub_objects_array;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<Bridge>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}