diff --git a/.github/workflows/BuildTest.yaml b/.github/workflows/BuildTest.yaml
index d807050c..996b3192 100644
--- a/.github/workflows/BuildTest.yaml
+++ b/.github/workflows/BuildTest.yaml
@@ -10,11 +10,11 @@ on:
 jobs:
   build_and_test:
     name: Build
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
-        rosdistro : [foxy, galactic]
+        rosdistro : [foxy, galactic, humble]
     steps:
       - uses: actions/checkout@v2-beta
       - name: Check dependency-${{ matrix.rosdistro }}.repos existence
@@ -45,4 +45,5 @@ jobs:
         uses: OUXT-Polaris/ros2-ci@master
         if: steps.check_rosdistro_repos_files.outputs.files_exists == 'false' && steps.check_repos_files.outputs.files_exists == 'false'
         with:
-          ros2-distro: ${{ matrix.rosdistro }}
\ No newline at end of file
+          ros2-distro: ${{ matrix.rosdistro }}
+          image-name: ros    # Docker Official Image
diff --git a/jsk_rviz_plugins/CMakeLists.txt b/jsk_rviz_plugins/CMakeLists.txt
index 75c67812..e20cdb20 100644
--- a/jsk_rviz_plugins/CMakeLists.txt
+++ b/jsk_rviz_plugins/CMakeLists.txt
@@ -19,6 +19,7 @@ find_package(rviz_ogre_vendor REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Widgets Test)
 find_package(image_transport REQUIRED)
 find_package(cv_bridge REQUIRED)
+find_package(Boost REQUIRED)
 
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -35,6 +36,7 @@ find_package(tf2_ros REQUIRED)
 find_package(urdf REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
+find_package(actionlib_msgs REQUIRED)
 
 find_package(jsk_footstep_msgs REQUIRED)
 find_package(jsk_gui_msgs REQUIRED)
@@ -44,7 +46,7 @@ find_package(jsk_rviz_plugin_msgs REQUIRED)
 find_package(jsk_topic_tools REQUIRED)
 
 # for Ogre include files
-include_directories(${ADDITIONAL_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS}/Paging)
+include_directories(${ADDITIONAL_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS}/Paging ${Boost_INCLUDE_DIRS})
 
 # Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
@@ -120,6 +122,7 @@ ament_target_dependencies(jsk_rviz_plugins
   geometry_msgs
   nav_msgs
   diagnostic_msgs
+  actionlib_msgs
   jsk_footstep_msgs
   jsk_gui_msgs
   jsk_hark_msgs
@@ -128,11 +131,11 @@ ament_target_dependencies(jsk_rviz_plugins
   jsk_topic_tools
   )
 
-target_include_directories(jsk_rviz_plugins
-  PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
-  )
+# target_include_directories(jsk_rviz_plugins
+#   PUBLIC
+#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#   $<INSTALL_INTERFACE:include>
+#   )
 
 if (NOT WIN32)
   ament_environment_hooks(
@@ -151,10 +154,9 @@ pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
 # replaces catkin_package(LIBRARIES jsk_rviz_plugins) 
 ament_export_libraries(jsk_rviz_plugins)
 
-ament_export_include_directories(include)
+# ament_export_include_directories(include)
 ament_export_targets(export_jsk_rviz_plugins HAS_LIBRARY_TARGET)
 ament_export_dependencies(
-  Qt5
   rviz_common
   cv_bridge
   image_transport
@@ -171,6 +173,7 @@ ament_export_dependencies(
   urdf
   visualization_msgs
   diagnostic_msgs
+  actionlib_msgs
   jsk_footstep_msgs
   jsk_gui_msgs
   jsk_hark_msgs
@@ -190,7 +193,7 @@ install(TARGETS
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
+  # INCLUDES DESTINATION include
   )
 
 install(DIRECTORY
diff --git a/jsk_rviz_plugins/package.xml b/jsk_rviz_plugins/package.xml
index 5e279051..06b94768 100644
--- a/jsk_rviz_plugins/package.xml
+++ b/jsk_rviz_plugins/package.xml
@@ -19,6 +19,7 @@
 
   <build_depend>qtbase5-dev</build_depend>
   <build_depend>rviz_ogre_vendor</build_depend>
+  <build_depend>boost</build_depend>
   <build_export_depend>rviz_ogre_vendor</build_export_depend>
 
   <exec_depend>libqt5-core</exec_depend>
@@ -46,6 +47,7 @@
   <depend>urdf</depend>
   <depend>visualization_msgs</depend>
   <depend>diagnostic_msgs</depend>
+  <depend>actionlib_msgs</depend>
   <depend>jsk_footstep_msgs</depend>
   <depend>jsk_gui_msgs</depend>
   <depend>jsk_hark_msgs</depend>
diff --git a/jsk_rviz_plugins/src/overlay_diagnostic_display.hpp b/jsk_rviz_plugins/src/overlay_diagnostic_display.hpp
index 51de9fe3..62ab8ddc 100644
--- a/jsk_rviz_plugins/src/overlay_diagnostic_display.hpp
+++ b/jsk_rviz_plugins/src/overlay_diagnostic_display.hpp
@@ -37,6 +37,7 @@
 #include <OgreTexture.h>
 
 #include <QPainter>
+#include <QPainterPath>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <rviz_common/properties/color_property.hpp>