diff --git a/README.md b/README.md
index bba57b1e..8b1c841d 100644
--- a/README.md
+++ b/README.md
@@ -20,6 +20,7 @@ See [read the docs](http://jsk-visualization.readthedocs.org/en/latest/).
 # 開発環境
 - Ubuntu 20.04(ROS2 Foxy)
 - Ubuntu 20.04(ROS2 Galactic)
+- Ubuntu 22.04(ROS2 Humble) - Compilation Only
 
 # 実装状況
 ## jsk_rviz_plugins
@@ -47,7 +48,7 @@ See [read the docs](http://jsk-visualization.readthedocs.org/en/latest/).
 | TargetVisualizer | ✅ | ✅ | |
 | Diagnostics | ✅ | ✅ | |
 | LinearGauge | ✅ | ✅ | ✅ | |
-| CameraInfo |  |  |  |
+| CameraInfo | ✅ | ✅ |  |
 | PoseArray | | | | (default pluginに実装済み?) |
 | OverlayCamera | | | | (Panelを含む? )|
 | PeoplePositionMeasuermentArray | | | | (msg未実装) |
diff --git a/jsk_rviz_plugins/CMakeLists.txt b/jsk_rviz_plugins/CMakeLists.txt
index e20cdb20..08e7306f 100644
--- a/jsk_rviz_plugins/CMakeLists.txt
+++ b/jsk_rviz_plugins/CMakeLists.txt
@@ -27,6 +27,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 
 find_package(laser_geometry REQUIRED)
+find_package(image_geometry REQUIRED)
 find_package(map_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(resource_retriever REQUIRED)
@@ -107,17 +108,22 @@ add_library(jsk_rviz_plugins SHARED
   src/close_all_tool.cpp
   src/open_all_tool.cpp
   src/screenshot_listener_tool.cpp
-  src/overlay_picker_tool.cpp)
+  src/overlay_picker_tool.cpp
 
   ### ViewController
   # src/view_controller/tablet_view_controller.cpp )
 
+  ### Properties
+  src/image_transport_hints_property.cpp
+
+)
 # Link ament packages
 ament_target_dependencies(jsk_rviz_plugins
   rclcpp
   rviz_common
   cv_bridge
   image_transport
+  image_geometry
   std_msgs
   geometry_msgs
   nav_msgs
@@ -137,12 +143,6 @@ ament_target_dependencies(jsk_rviz_plugins
 #   $<INSTALL_INTERFACE:include>
 #   )
 
-if (NOT WIN32)
-  ament_environment_hooks(
-    "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}"
-  )
-endif()
-
 # Link non ament packages
 target_link_libraries(jsk_rviz_plugins Qt5::Widgets)
 
@@ -162,6 +162,7 @@ ament_export_dependencies(
   image_transport
   geometry_msgs
   laser_geometry
+  image_geometry
   map_msgs
   nav_msgs
   rclcpp
diff --git a/jsk_rviz_plugins/package.xml b/jsk_rviz_plugins/package.xml
index 06b94768..e8dc0dfd 100644
--- a/jsk_rviz_plugins/package.xml
+++ b/jsk_rviz_plugins/package.xml
@@ -14,6 +14,7 @@
   <author email="youhei@jsk.t.u-tokyo.ac.jp">Yohei Kakiuchi</author>
   <author email="s-fujii@jsk.t.u-tokyo.ac.jp">Shohei Fujii</author>
   <author email="ueda@jsk.t.u-tokyo.ac.jp">Ryohei Ueda</author>
+  <author email="ander.gonzalez@ikerlan.es">Ander Gonzalez</author> <!-- CameraInfo Display -->
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -34,6 +35,7 @@
   <depend>geometry_msgs</depend>
   <!-- <depend>interactive_markers</depend> -->
   <depend>laser_geometry</depend>
+  <depend>image_geometry</depend>
   <depend>nav_msgs</depend>
   <depend>map_msgs</depend>
   <depend>pluginlib</depend>
diff --git a/jsk_rviz_plugins/plugins_description.xml b/jsk_rviz_plugins/plugins_description.xml
index a511b5bc..367c6b4a 100644
--- a/jsk_rviz_plugins/plugins_description.xml
+++ b/jsk_rviz_plugins/plugins_description.xml
@@ -76,13 +76,13 @@
   </class>
 
   
-  <!-- <class name="jsk_rviz_plugin/CameraInfo"
+  <class name="jsk_rviz_plugin/CameraInfo"
          type="jsk_rviz_plugins::CameraInfoDisplay"
          base_class_type="rviz_common::Display">
     <description>
       visualize sensor_msgs/CameraInfo
     </description>
-  </class> -->
+  </class>
 
   <!-- <class name="jsk_rviz_plugin/PeoplePositionMeasurementArray"
          type="jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay"
diff --git a/jsk_rviz_plugins/src/camera_info_display.cpp b/jsk_rviz_plugins/src/camera_info_display.cpp
index 93b001e6..9e0dca86 100644
--- a/jsk_rviz_plugins/src/camera_info_display.cpp
+++ b/jsk_rviz_plugins/src/camera_info_display.cpp
@@ -28,586 +28,569 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-// #include <rviz_common/uniform_string_stream.hpp>
-// //#include <image_transport/image_transport.hpp>
-// #include <camera_info_display.hpp>
-// #include <OgreMaterialManager.h>
-// #include <OgreMaterial.h>
-// #include <OgreBlendMode.h>
-// #include <QImage>
-// #include <OgreHardwarePixelBuffer.h>
-
-// namespace jsk_rviz_plugins
-// {
-//   TrianglePolygon::TrianglePolygon(
-//     Ogre::SceneManager* manager,
-//     Ogre::SceneNode* node,
-//     const cv::Point3d& O,
-//     const cv::Point3d& A,
-//     const cv::Point3d& B,
-//     const std::string& name,
-//     const Ogre::ColourValue& color,
-//     bool use_color,
-//     bool upper_triangle)
-//   {
-//     // uniq string is requred for name
-
-//     manual_ = manager->createManualObject();
-//     manual_->clear();
-//     manual_->begin(name,
-//                    Ogre::RenderOperation::OT_TRIANGLE_STRIP);
-//     manual_->position(O.x, O.y, O.z);
-//     if (upper_triangle) {
-//       manual_->textureCoord(0, 0);
-//     }
-//     else {
-//       manual_->textureCoord(1, 0);
-//     }
-//     if (use_color) {
-//       manual_->colour(color);
-//     }
-//     manual_->position(A.x, A.y, A.z);
-//     if (upper_triangle) {
-//       manual_->textureCoord(1, 0);
-//     }
-//     else {
-//       manual_->textureCoord(1, 1);
-//     }
-//     if (use_color) {
-//       manual_->colour(color);
-//     }
-//     manual_->position(B.x, B.y, B.z);
-//     if (upper_triangle) {
-//       manual_->textureCoord(0, 1);
-//     }
-//     else {
-//       manual_->textureCoord(0, 1);
-//     }
-//     if (use_color) {
-//       manual_->colour(color);
-//     }
-//     manual_->end();
-//     node->attachObject(manual_);
-//   }
-
-//   TrianglePolygon::~TrianglePolygon()
-//   {
-//     manual_->detachFromParent();
-//     //manager_->destroyManualObject(manual_); // this crashes rviz
-//   }
-
-//   CameraInfoDisplay::CameraInfoDisplay(): image_updated_(true)
-//   {
-//     ////////////////////////////////////////////////////////
-//     // initialize properties
-//     ////////////////////////////////////////////////////////
-//     far_clip_distance_property_ = new rviz_common::properties::FloatProperty(
-//       "far clip",
-//       1.0,
-//       "far clip distance from the origin of camera info",
-//       this, SLOT(updateFarClipDistance()));
-//     show_edges_property_ = new rviz_common::properties::BoolProperty(
-//       "show edges",
-//       true,
-//       "show edges of the region of the camera info",
-//       this, SLOT(updateShowEdges()));
-//     show_polygons_property_ = new rviz_common::properties::BoolProperty(
-//       "show polygons",
-//       true,
-//       "show polygons of the region of the camera info",
-//       this, SLOT(updateShowPolygons()));
-//     not_show_side_polygons_property_ = new rviz_common::properties::BoolProperty(
-//       "not show side polygons",
-//       true,
-//       "do not show polygons of the region of the camera info",
-//       this, SLOT(updateNotShowSidePolygons()));
-//     use_image_property_ = new rviz_common::properties::BoolProperty(
-//       "use image",
-//       false,
-//       "use image as texture",
-//       this, SLOT(updateUseImage()));
-//     image_topic_property_->hide();
-//     image_transport_hints_property_ = new ImageTransportHintsProperty(
-//       "transport hints",
-//       "transport hint for image subscription",
-//       this, SLOT( updateImageTopic() ));
-//     image_transport_hints_property_->hide();
-
-//     color_property_ = new rviz_common::properties::ColorProperty(
-//       "color",
-//       QColor(85, 255, 255),
-//       "color of CameraInfo",
-//       this, SLOT(updateColor()));
-//     edge_color_property_ = new rviz_common::properties::ColorProperty(
-//       "edge color",
-//       QColor(125, 125, 125),
-//       "edge color of CameraInfo",
-//       this, SLOT(updateEdgeColor()));
-//     alpha_property_ = new rviz_common::properties::FloatProperty(
-//       "alpha",
-//       0.5,
-//       "alpha blending value",
-//       this, SLOT(updateAlpha()));
-//   }
-
-//   CameraInfoDisplay::~CameraInfoDisplay()
-//   {
-//     if (edges_) {
-//       edges_->clear();
-//     }
-//     polygons_.clear();
-//     delete far_clip_distance_property_;
-//     delete color_property_;
-//     delete alpha_property_;
-//     delete show_polygons_property_;
-//     delete edge_color_property_;
-//   }
-
-//   void CameraInfoDisplay::reset()
-//   {
-//     RTDClass::reset();
-//     if (edges_) {
-//       edges_->clear();
-//     }
-//     polygons_.clear();
-//     camera_info_ = sensor_msgs::msg::CameraInfo::ConstPtr(); // reset to NULL
-//   }
-
-//   void CameraInfoDisplay::onInitialize()
-//   {
-//     RTDClass::onInitialize();
-//     scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
-//     updateColor();
-//     updateAlpha();
-//     updateFarClipDistance();
-//     updateShowPolygons();
-//     updateNotShowSidePolygons();
-//     updateShowEdges();
-//     updateImageTopic();
-//     updateUseImage();
-//     updateEdgeColor();
-//   }
-
-//   void CameraInfoDisplay::processMessage(
-//     sensor_msgs::msg::CameraInfo::ConstSharedPtr msg)
-//   {
-//     if (!isSameCameraInfo(msg)) {
-//       createCameraInfoShapes(msg);
-//     }
-//     // move scene_node according to tf
-//      Ogre::Vector3 position;
-//      Ogre::Quaternion quaternion;
-//      if(!context_->getFrameManager()->getTransform(msg->header.frame_id,
-//                                                    msg->header.stamp,
-//                                                    position,
-//                                                    quaternion)) {
-//        ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
-//                   qPrintable( getName() ), msg->header.frame_id.c_str(),
-//                   qPrintable( fixed_frame_ ));
-//      }
-//      scene_node_->setPosition(position);
-//      scene_node_->setOrientation(quaternion);
-//      camera_info_ = msg;        // store for caching
-//   }
-
-//   void CameraInfoDisplay::update(float wall_dt, float ros_dt)
-//   {
-//     std::lock_guard<std::mutex> lock(mutex_);
-//     if (image_updated_) {
-//       ROS_DEBUG("image updated");
-//       if (!bottom_texture_.isNull()) {
-//         drawImageTexture();
-//         image_updated_ = false;
-//       }
-//     }
-//   }
-
-//   bool CameraInfoDisplay::isSameCameraInfo(
-//     sensor_msgs::msg::CameraInfo::ConstSharedPtr msg)
-//   {
-//     if (camera_info_) {
-//       bool meta_same_p =
-//         msg->header.frame_id == camera_info_->header.frame_id &&
-//         msg->height == camera_info_->height &&
-//         msg->width == camera_info_->width &&
-//         msg->distortion_model == camera_info_->distortion_model;
-//       if (meta_same_p) {
-//         for (size_t i = 0; i < msg->P.size(); i++) {
-//           if (msg->P[i] != camera_info_->P[i]) {
-//             return false;
-//           }
-//         }
-//         return true;
-//       }
-//       else {
-//         return false;
-//       }
-//     }
-//     else {
-//       return false;
-//     }
-//   }
-
-//   void CameraInfoDisplay::addPointToEdge(
-//     const cv::Point3d& point)
-//   {
-//     Ogre::Vector3 p;
-//     p[0] = point.x;
-//     p[1] = point.y;
-//     p[2] = point.z;
-//     edges_->addPoint(p);
-//   }
-
-//   void CameraInfoDisplay::addPolygon(
-//     const cv::Point3d& O, const cv::Point3d& A, const cv::Point3d& B, std::string name, bool use_color, bool upper_triangle)
-//   {
-//     Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
-//     color.a = alpha_;
-//     TrianglePolygon::Ptr triangle (new TrianglePolygon(
-//                                      scene_manager_,
-//                                      scene_node_,
-//                                      O, A, B, name,
-//                                      color,
-//                                      use_color,
-//                                      upper_triangle));
-//     polygons_.push_back(triangle);
-//   }
-
-//   void CameraInfoDisplay::createTextureForBottom(int width, int height)
-//   {
-//     if (bottom_texture_.isNull()
-//         || bottom_texture_->getWidth() != width
-//         || bottom_texture_->getHeight() != height) {
-//       static uint32_t count = 0;
-//       rviz_common::UniformStringStream ss;
-//       ss << "CameraInfoDisplayPolygonBottom" << count++;
-//       material_bottom_
-//         = Ogre::MaterialManager::getSingleton().create(
-//           ss.str(),
-//           Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-//       bottom_texture_ = Ogre::TextureManager::getSingleton().createManual(
-//         material_bottom_->getName() + "Texture",        // name
-//         Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
-//         Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT);
-//       material_bottom_->getTechnique(0)->getPass(0)->setColourWriteEnabled(true);
-//       Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
-//       color.a = alpha_;
-//       material_bottom_->getTechnique(0)->getPass(0)->setAmbient(color);
-//       material_bottom_->setReceiveShadows(false);
-//       material_bottom_->getTechnique(0)->setLightingEnabled(true);
-//       material_bottom_->getTechnique(0)->getPass(0)->setCullingMode(Ogre::CULL_NONE);
-//       material_bottom_->getTechnique(0)->getPass(0)->setLightingEnabled(false);
-//       material_bottom_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
-//       material_bottom_->getTechnique(0)->getPass(0)->setDepthCheckEnabled(true);
-
-//       material_bottom_->getTechnique(0)->getPass(0)->setVertexColourTracking(Ogre::TVC_DIFFUSE);
-//       material_bottom_->getTechnique(0)->getPass(0)->createTextureUnitState(bottom_texture_->getName());
-//       material_bottom_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
-//     }
-//   }
-
-//   void CameraInfoDisplay::prepareMaterial()
-//   {
-//     if (texture_.isNull()) {
-//       // material
-//       static uint32_t count = 0;
-//       rviz_common::UniformStringStream ss;
-//       ss << "CameraInfoDisplayPolygon" << count++;
-//       material_
-//         = Ogre::MaterialManager::getSingleton().create(
-//           ss.str(),
-//           Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-//       texture_ = Ogre::TextureManager::getSingleton().createManual(
-//         material_->getName() + "Texture",        // name
-//         Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
-//         Ogre::TEX_TYPE_2D, 1, 1, 0, Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT);
-//       material_->getTechnique(0)->getPass(0)->setColourWriteEnabled(true);
-//       Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
-//       color.a = alpha_;
-//       material_->getTechnique(0)->getPass(0)->setAmbient(color);
-//       material_->setReceiveShadows(false);
-//       material_->getTechnique(0)->setLightingEnabled(true);
-//       material_->getTechnique(0)->getPass(0)->setCullingMode(Ogre::CULL_NONE);
-//       material_->getTechnique(0)->getPass(0)->setLightingEnabled(false);
-//       material_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
-//       material_->getTechnique(0)->getPass(0)->setDepthCheckEnabled(true);
-
-//       material_->getTechnique(0)->getPass(0)->setVertexColourTracking(Ogre::TVC_DIFFUSE);
-//       material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_->getName());
-//       material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
-//       createTextureForBottom(640, 480);
-//     }
-//   }
-
-//   // void CameraInfoDisplay::subscribeImage(std::string topic)
-//   // {
-//   //   image_sub_.shutdown();
-//   //   if (topic.empty()) {
-//   //     ROS_WARN("topic name is empty");
-//   //   }
-//   //   ros::NodeHandle nh;
-//   //   image_transport::ImageTransport it(nh);
-//   //   image_sub_ = it.subscribe(topic, 1, &CameraInfoDisplay::imageCallback, this,
-//   //                             image_transport_hints_property_->getTransportHints());
-//   // }
-
-//   void CameraInfoDisplay::drawImageTexture()
-//   {
-//     bottom_texture_->getBuffer()->lock( Ogre::HardwareBuffer::HBL_NORMAL );
-//     const Ogre::PixelBox& pixelBox
-//       = bottom_texture_->getBuffer()->getCurrentLock();
-//     Ogre::uint8* pDest = static_cast<Ogre::uint8*> (pixelBox.data);
-
-//     // Don't copy pixel-by-pixel image matrices.
-//     // Just split matrix into channels, add needed alpha channel and merge back directly into buffer.
-//     if (use_image_ && !image_.empty() &&
-//         bottom_texture_->getHeight() == image_.rows &&
-//         bottom_texture_->getWidth() == image_.cols) {
-//       ROS_DEBUG("bottom_texture_->getHeight(): %u", bottom_texture_->getHeight());
-//       ROS_DEBUG("bottom_texture_->getWidth(): %u", bottom_texture_->getWidth());
-//       ROS_DEBUG("image_.rows: %d", image_.rows);
-//       ROS_DEBUG("image_.cols: %d", image_.cols);
-
-//       std::vector<cv::Mat> splitted;
-//       cv::split(image_, splitted);
-//       // Swap channels RGB -> BGR for cv::merge.
-//       std::swap(splitted[0], splitted[2]);
-//       cv::Mat alpha(image_.rows, image_.cols, CV_8U, cv::Scalar(alpha_ * 255.0));
-//       splitted.push_back(alpha);
-//       cv::Mat boxMat(image_.rows, image_.cols, CV_8UC4, pDest);
-//       cv::merge(splitted, boxMat);
-//     } else {
-//       memset(pDest, 0, bottom_texture_->getWidth() * bottom_texture_->getHeight());
-//       QImage Hud(pDest, bottom_texture_->getWidth(), bottom_texture_->getHeight(), QImage::Format_ARGB32);
-//       for (size_t j = 0; j < bottom_texture_->getHeight(); j++) {
-//         for (size_t i = 0; i < bottom_texture_->getWidth(); i++) {
-//           Hud.setPixel(i, j, color_.rgba());
-//         }
-//       }
-//     }
-//     bottom_texture_->getBuffer()->unlock();
-//   }
-
-//   // convert sensor_msgs::Image into cv::Mat
-//   void CameraInfoDisplay::imageCallback(
-//       const sensor_msgs::Image::ConstPtr& msg)
-//   {
-//     std::lock_guard<std::mutex> lock(mutex_);
-//     cv_bridge::CvImagePtr cv_ptr;
-//     try
-//     {
-//       cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
-//       image_ = cv_ptr->image;
-//       // check the size of bottom texture
-//       if (bottom_texture_.isNull()
-//           || bottom_texture_->getWidth() != image_.cols
-//           || bottom_texture_->getHeight() != image_.rows) {
-//         createTextureForBottom(image_.cols, image_.rows);
-//         if (camera_info_) {
-//           createCameraInfoShapes(camera_info_);
-//         }
-//       }
-//       image_updated_ = true;
-//     }
-//     catch (cv_bridge::Exception& e)
-//     {
-//       ROS_ERROR("cv_bridge exception: %s", e.what());
-//     }
-//   }
-
-//   void CameraInfoDisplay::createCameraInfoShapes(
-//     sensor_msgs::msg::CameraInfo::ConstSharedPtr msg)
-//   {
-//     polygons_.clear();
-//     if (edges_) {
-//       edges_->clear();
-//     }
-//     image_geometry::PinholeCameraModel model;
-//     bool model_success_p = model.fromCameraInfo(msg);
-//     if (!model_success_p) {
-//       setStatus(rviz_common::properties::StatusProperty::Error, "Camera Info", "Failed to create camera model from msg");
-//       ROS_ERROR("failed to create camera model");
-//       return;
-//     }
-//     // fx and fy should not be equal 0.
-//     if (model.fx() == 0.0 || model.fy() == 0.0) {
-//       setStatus(rviz_common::properties::StatusProperty::Error, "Camera Info", "Invalid intrinsic matrix");
-//       ROS_ERROR_STREAM("camera model have invalid intrinsic matrix " << model.intrinsicMatrix());
-//       return;
-//     }
-//     setStatus(rviz_common::properties::StatusProperty::Ok, "Camera Info", "OK");
-
-//     ////////////////////////////////////////////////////////
-//     // initialize BillboardLine
-//     ////////////////////////////////////////////////////////
-//     if (!edges_) {
-//       edges_.reset(new rviz_rendering::BillboardLine(context_->getSceneManager(),
-//                                            scene_node_));
-//       edges_->setLineWidth(0.01);
-//     }
-
-//     cv::Point2d a(0, 0), b(msg->width, 0),
-//       c(msg->width, msg->height), d(0, msg->height);
-//     // all the z = 1.0
-//     cv::Point3d A = model.projectPixelTo3dRay(a);
-//     cv::Point3d B = model.projectPixelTo3dRay(b);
-//     cv::Point3d C = model.projectPixelTo3dRay(c);
-//     cv::Point3d D = model.projectPixelTo3dRay(d);
-
-//     cv::Point3d scaled_A = A * far_clip_distance_;
-//     cv::Point3d scaled_B = B * far_clip_distance_;
-//     cv::Point3d scaled_C = C * far_clip_distance_;
-//     cv::Point3d scaled_D = D * far_clip_distance_;
-
-//     cv::Point3d O(0, 0, 0);
-
-//     ////////////////////////////////////////////////////////
-//     // build polygons
-//     ////////////////////////////////////////////////////////
-//     if (show_polygons_) {
-//       ////////////////////////////////////////////////////////
-//       // setup color for polygons
-//       ////////////////////////////////////////////////////////
-//       Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
-//       color.a = alpha_;
-//       prepareMaterial();
-//       if (!not_show_side_polygons_) {
-//         material_->getTechnique(0)->getPass(0)->setAmbient(color);
-//         {
-//           texture_->getBuffer()->lock( Ogre::HardwareBuffer::HBL_NORMAL );
-//           const Ogre::PixelBox& pixelBox
-//             = texture_->getBuffer()->getCurrentLock();
-//           Ogre::uint8* pDest = static_cast<Ogre::uint8*> (pixelBox.data);
-//           memset(pDest, 0, 1);
-//           QImage Hud(pDest, 1, 1, QImage::Format_ARGB32 );
-//           Hud.setPixel(0, 0, color_.rgba());
-//           texture_->getBuffer()->unlock();
-//         }
-//         addPolygon(O, scaled_B, scaled_A, material_->getName(), true, true);
-//         addPolygon(O, scaled_C, scaled_B, material_->getName(), true, true);
-//         addPolygon(O, scaled_D, scaled_C, material_->getName(), true, true);
-//         addPolygon(O, scaled_A, scaled_D, material_->getName(), true, true);
-//       }
-//       // bottom
-//       drawImageTexture();
-
-//       addPolygon(scaled_A, scaled_B, scaled_D, material_bottom_->getName(), false, true);
-//       addPolygon(scaled_B, scaled_C, scaled_D, material_bottom_->getName(), false, false);
-//     }
-//     ////////////////////////////////////////////////////////
-//     // build edges
-//     ////////////////////////////////////////////////////////
-//     if (show_edges_) {
-//       edges_->clear();
-//       edges_->setMaxPointsPerLine(2);
-//       edges_->setNumLines(8);
-//       edges_->setColor(edge_color_.red() / 255.0,
-//                        edge_color_.green() / 255.0,
-//                        edge_color_.blue() / 255.0,
-//                        alpha_);
-//       addPointToEdge(O); addPointToEdge(scaled_A); edges_->newLine();
-//       addPointToEdge(O); addPointToEdge(scaled_B); edges_->newLine();
-//       addPointToEdge(O); addPointToEdge(scaled_C); edges_->newLine();
-//       addPointToEdge(O); addPointToEdge(scaled_D); edges_->newLine();
-//       addPointToEdge(scaled_A); addPointToEdge(scaled_B); edges_->newLine();
-//       addPointToEdge(scaled_B); addPointToEdge(scaled_C); edges_->newLine();
-//       addPointToEdge(scaled_C); addPointToEdge(scaled_D); edges_->newLine();
-//       addPointToEdge(scaled_D); addPointToEdge(scaled_A);
-//     }
-//   }
-
-//   ////////////////////////////////////////////////////////
-//   // Properties updating functions
-//   ////////////////////////////////////////////////////////
-//   void CameraInfoDisplay::updateColor()
-//   {
-//     color_ = color_property_->getColor();
-//     if (camera_info_) {
-//       createCameraInfoShapes(camera_info_);
-//     }
-//   }
-
-//   void CameraInfoDisplay::updateEdgeColor()
-//   {
-//     edge_color_ = edge_color_property_->getColor();
-//     if (camera_info_) {
-//       createCameraInfoShapes(camera_info_);
-//     }
-//   }
-
-//   void CameraInfoDisplay::updateAlpha()
-//   {
-//     alpha_ = alpha_property_->getFloat();
-//     if (camera_info_) {
-//       createCameraInfoShapes(camera_info_);
-//     }
-//   }
-
-//   void CameraInfoDisplay::updateFarClipDistance()
-//   {
-//     far_clip_distance_ = far_clip_distance_property_->getFloat();
-//     if (camera_info_) {
-//       createCameraInfoShapes(camera_info_);
-//     }
-//   }
-
-//   void CameraInfoDisplay::updateShowPolygons()
-//   {
-//     show_polygons_ = show_polygons_property_->getBool();
-//     if (show_polygons_) {
-//       not_show_side_polygons_property_->show();
-//     }
-//     else {
-//       not_show_side_polygons_property_->hide();
-//     }
-//     if (camera_info_) {
-//       createCameraInfoShapes(camera_info_);
-//     }
-//   }
-
-//   void CameraInfoDisplay::updateShowEdges()
-//   {
-//     show_edges_ = show_edges_property_->getBool();
-//     if (camera_info_) {
-//       createCameraInfoShapes(camera_info_);
-//     }
-//   }
-
-//   void CameraInfoDisplay::updateImageTopic()
-//   {
-//     if (use_image_) {
-//       std::string topic = image_topic_property_->getStdString();
-//       //subscribeImage(topic);
-//       subscribe();
-//     } else {
-//       unsubscribe();
-//       //image_sub_.shutdown();
-//       // Set image_updated_ true in order to clear the bottom texture in update() method.
-//       image_updated_ = true;
-//     }
-//   }
-
-//   void CameraInfoDisplay::updateUseImage()
-//   {
-//     use_image_ = use_image_property_->getBool();
-//     if (use_image_) {
-//       image_topic_property_->show();
-//       image_transport_hints_property_->show();
-//     }
-//     else {
-//       image_topic_property_->hide();
-//       image_transport_hints_property_->hide();
-//     }
-//     updateImageTopic();
-//   }
-//   void CameraInfoDisplay::updateNotShowSidePolygons()
-//   {
-//     not_show_side_polygons_ = not_show_side_polygons_property_->getBool();
-//     if (camera_info_) {
-//       createCameraInfoShapes(camera_info_);
-//     }
-//   }
-// }
-
-// #include <pluginlib/class_list_macros.hpp>
-// PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::CameraInfoDisplay, rviz_common::Display )
+// ROS 2 Port:
+// Ander González Tomé <ander.gonzalez@ikerlan.es> @ IKERLAN S. COOP.
+
+#include "camera_info_display.hpp"
+
+#include <OgreBlendMode.h>
+#include <OgreHardwarePixelBuffer.h>
+#include <OgreMaterial.h>
+#include <OgreMaterialManager.h>
+#include <OgreTechnique.h>
+#include <image_geometry/pinhole_camera_model.hpp>
+
+#include <QImage>
+#include <image_transport/image_transport.hpp>
+#include <rviz_common/properties/parse_color.hpp>
+#include <rviz_common/uniform_string_stream.hpp>
+#include <utility>
+#include <vector>
+
+namespace jsk_rviz_plugins
+{
+TrianglePolygon::TrianglePolygon(
+  Ogre::SceneManager * manager, Ogre::SceneNode * node, const cv::Point3d & O,
+  const cv::Point3d & A, const cv::Point3d & B, const std::string & name,
+  const Ogre::ColourValue & color, bool use_color, bool upper_triangle)
+{
+  // uniq string is requred for name
+
+  manual_ = manager->createManualObject();
+  manual_->clear();
+  manual_->begin(name, Ogre::RenderOperation::OT_TRIANGLE_STRIP);
+  manual_->position(O.x, O.y, O.z);
+  if (upper_triangle) {
+    manual_->textureCoord(0, 0);
+  } else {
+    manual_->textureCoord(1, 0);
+  }
+  if (use_color) {
+    manual_->colour(color);
+  }
+  manual_->position(A.x, A.y, A.z);
+  if (upper_triangle) {
+    manual_->textureCoord(1, 0);
+  } else {
+    manual_->textureCoord(1, 1);
+  }
+  if (use_color) {
+    manual_->colour(color);
+  }
+  manual_->position(B.x, B.y, B.z);
+  if (upper_triangle) {
+    manual_->textureCoord(0, 1);
+  } else {
+    manual_->textureCoord(0, 1);
+  }
+  if (use_color) {
+    manual_->colour(color);
+  }
+  manual_->end();
+  node->attachObject(manual_);
+}
+
+TrianglePolygon::~TrianglePolygon()
+{
+  manual_->detachFromParent();
+  // manager_->destroyManualObject(manual_); // this crashes rviz
+}
+
+CameraInfoDisplay::CameraInfoDisplay() : image_updated_(true)
+{
+  ////////////////////////////////////////////////////////
+  // initialize properties
+  ////////////////////////////////////////////////////////
+  far_clip_distance_property_ = new rviz_common::properties::FloatProperty(
+    "far clip", 1.0, "far clip distance from the origin of camera info", this,
+    SLOT(updateFarClipDistance()));
+  show_edges_property_ = new rviz_common::properties::BoolProperty(
+    "show edges", true,
+    "show edges of the region of "
+    "the camera info",
+    this, SLOT(updateShowEdges()));
+  show_polygons_property_ = new rviz_common::properties::BoolProperty(
+    "show polygons", true, "show polygons of the region of the camera info", this,
+    SLOT(updateShowPolygons()));
+  not_show_side_polygons_property_ = new rviz_common::properties::BoolProperty(
+    "not show side polygons", true,
+    "do not show polygons of the region of "
+    "the camera info",
+    this, SLOT(updateNotShowSidePolygons()));
+  use_image_property_ = new rviz_common::properties::BoolProperty(
+    "use image", false, "use image as texture", this, SLOT(updateUseImage()));
+  image_topic_property_ = new rviz_common::properties::RosTopicProperty(
+    "Image Topic", "", rosidl_generator_traits::name<sensor_msgs::msg::Image>(),
+    "sensor_msgs::Image topic to subscribe to.", this, SLOT(updateImageTopic()));
+  image_topic_property_->setHidden(true);
+
+  image_transport_hints_property_ = new ImageTransportHintsProperty(
+    "Transport Type", "transport hint for image subscription", this, SLOT(updateImageTopic()));
+  image_transport_hints_property_->hide();
+  color_property_ = new rviz_common::properties::ColorProperty(
+    "color", QColor(85, 255, 255), "color of CameraInfo", this, SLOT(updateColor()));
+  edge_color_property_ = new rviz_common::properties::ColorProperty(
+    "edge color", QColor(125, 125, 125), "edge color of CameraInfo", this, SLOT(updateEdgeColor()));
+  alpha_property_ = new rviz_common::properties::FloatProperty(
+    "alpha", 0.5, "alpha blending value", this, SLOT(updateAlpha()));
+}
+
+CameraInfoDisplay::~CameraInfoDisplay()
+{
+  if (edges_) {
+    edges_->clear();
+  }
+  polygons_.clear();
+  delete far_clip_distance_property_;
+  delete color_property_;
+  delete alpha_property_;
+  delete show_polygons_property_;
+  delete edge_color_property_;
+}
+
+void CameraInfoDisplay::reset()
+{
+  RTDClass::reset();
+  if (edges_) {
+    edges_->clear();
+  }
+  polygons_.clear();
+  camera_info_ = sensor_msgs::msg::CameraInfo::ConstPtr();  // reset to NULL
+}
+
+void CameraInfoDisplay::onInitialize()
+{
+  RTDClass::onInitialize();
+  node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();
+  image_topic_property_->initialize(context_->getRosNodeAbstraction());
+  RCLCPP_DEBUG(node_->get_logger(), "Initializing CameraInfo view");
+  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+  updateColor();
+  updateAlpha();
+  updateFarClipDistance();
+  updateShowPolygons();
+  updateNotShowSidePolygons();
+  updateShowEdges();
+  updateImageTopic();
+  updateUseImage();
+  updateEdgeColor();
+
+  RCLCPP_DEBUG(node_->get_logger(), "Initialized CameraInfo view");
+}
+
+void CameraInfoDisplay::processMessage(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg)
+{
+  if (!isSameCameraInfo(msg)) {
+    createCameraInfoShapes(msg);
+  }
+  // move scene_node according to tf
+  Ogre::Vector3 position;
+  Ogre::Quaternion quaternion;
+  if (!context_->getFrameManager()->getTransform(
+        msg->header.frame_id, msg->header.stamp, position, quaternion)) {
+    RCLCPP_ERROR(
+      node_->get_logger(), "Error transforming pose '%s' from frame '%s' to frame '%s'",
+      qPrintable(getName()), msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
+  }
+  scene_node_->setPosition(position);
+  scene_node_->setOrientation(quaternion);
+  camera_info_ = msg;  // store for caching
+}
+
+void CameraInfoDisplay::update(float wall_dt, float ros_dt)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (image_updated_) {
+    if (bottom_texture_) {
+      drawImageTexture();
+      image_updated_ = false;
+    }
+  }
+}
+
+bool CameraInfoDisplay::isSameCameraInfo(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg)
+{
+  if (camera_info_) {
+    bool meta_same_p = msg->header.frame_id == camera_info_->header.frame_id &&
+                       msg->height == camera_info_->height && msg->width == camera_info_->width &&
+                       msg->distortion_model == camera_info_->distortion_model;
+    if (meta_same_p) {
+      for (size_t i = 0; i < msg->p.size(); i++) {
+        if (msg->p[i] != camera_info_->p[i]) {
+          return false;
+        }
+      }
+      return true;
+    } else {
+      return false;
+    }
+  } else {
+    return false;
+  }
+}
+
+void CameraInfoDisplay::addPointToEdge(const cv::Point3d & point)
+{
+  Ogre::Vector3 p;
+  p[0] = point.x;
+  p[1] = point.y;
+  p[2] = point.z;
+  edges_->addPoint(p);
+}
+
+void CameraInfoDisplay::addPolygon(
+  const cv::Point3d & O, const cv::Point3d & A, const cv::Point3d & B, std::string name,
+  bool use_color, bool upper_triangle)
+{
+  Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
+  color.a = alpha_;
+  TrianglePolygon::Ptr triangle(new TrianglePolygon(
+    scene_manager_, scene_node_, O, A, B, name, color, use_color, upper_triangle));
+  polygons_.push_back(triangle);
+}
+
+void CameraInfoDisplay::createTextureForBottom(int width, int height)
+{
+  if (
+    !bottom_texture_ || bottom_texture_->getWidth() != width ||
+    bottom_texture_->getHeight() != height) {
+    static uint32_t count = 0;
+    rviz_common::UniformStringStream ss;
+    ss << "CameraInfoDisplayPolygonBottom" << count++;
+    material_bottom_ = Ogre::MaterialManager::getSingleton().create(
+      ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+    bottom_texture_ = Ogre::TextureManager::getSingleton().createManual(
+      material_bottom_->getName() + "Texture",  // name
+      Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, width, height, 0,
+      Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT);
+    material_bottom_->getTechnique(0)->getPass(0)->setColourWriteEnabled(true);
+    Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
+    color.a = alpha_;
+    material_bottom_->getTechnique(0)->getPass(0)->setAmbient(color);
+    material_bottom_->setReceiveShadows(false);
+    material_bottom_->getTechnique(0)->setLightingEnabled(true);
+    material_bottom_->getTechnique(0)->getPass(0)->setCullingMode(Ogre::CULL_NONE);
+    material_bottom_->getTechnique(0)->getPass(0)->setLightingEnabled(false);
+    material_bottom_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
+    material_bottom_->getTechnique(0)->getPass(0)->setDepthCheckEnabled(true);
+
+    material_bottom_->getTechnique(0)->getPass(0)->setVertexColourTracking(Ogre::TVC_DIFFUSE);
+    material_bottom_->getTechnique(0)->getPass(0)->createTextureUnitState(
+      bottom_texture_->getName());
+    material_bottom_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+  }
+}
+
+void CameraInfoDisplay::prepareMaterial()
+{
+  if (!texture_) {
+    // material
+    static uint32_t count = 0;
+    rviz_common::UniformStringStream ss;
+    ss << "CameraInfoDisplayPolygon" << count++;
+    material_ = Ogre::MaterialManager::getSingleton().create(
+      ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+    texture_ = Ogre::TextureManager::getSingleton().createManual(
+      material_->getName() + "Texture",  // name
+      Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, 1, 1, 0,
+      Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT);
+    material_->getTechnique(0)->getPass(0)->setColourWriteEnabled(true);
+    Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
+    color.a = alpha_;
+    material_->getTechnique(0)->getPass(0)->setAmbient(color);
+    material_->setReceiveShadows(false);
+    material_->getTechnique(0)->setLightingEnabled(true);
+    material_->getTechnique(0)->getPass(0)->setCullingMode(Ogre::CULL_NONE);
+    material_->getTechnique(0)->getPass(0)->setLightingEnabled(false);
+    material_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
+    material_->getTechnique(0)->getPass(0)->setDepthCheckEnabled(true);
+
+    material_->getTechnique(0)->getPass(0)->setVertexColourTracking(Ogre::TVC_DIFFUSE);
+    material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_->getName());
+    material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+    createTextureForBottom(640, 480);
+  }
+}
+
+void CameraInfoDisplay::subscribeImage(std::string topic)
+{
+  image_sub_.shutdown();
+  if (topic.empty()) {
+    RCLCPP_DEBUG_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 5000, "CameraInfo topic name is empty");
+    return;
+  }
+  image_transport::ImageTransport it(node_);
+  auto image_properties = image_transport_hints_property_->getTransportHints(node_.get());
+  image_sub_ = it.subscribe(topic, 1, &CameraInfoDisplay::imageCallback, this, &image_properties);
+}
+
+void CameraInfoDisplay::drawImageTexture()
+{
+  bottom_texture_->getBuffer()->lock(Ogre::HardwareBuffer::HBL_NORMAL);
+  const Ogre::PixelBox & pixelBox = bottom_texture_->getBuffer()->getCurrentLock();
+  Ogre::uint8 * pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
+  // Don't copy pixel-by-pixel image matrices.
+  // Just split matrix into channels
+  // add needed alpha channel and merge back directly into buffer.
+  if (
+    use_image_ && !image_.empty() && bottom_texture_->getHeight() == image_.rows &&
+    bottom_texture_->getWidth() == image_.cols) {
+    RCLCPP_DEBUG(
+      node_->get_logger(), "bottom_texture_->getHeight(): %u", bottom_texture_->getHeight());
+    RCLCPP_DEBUG(
+      node_->get_logger(), "bottom_texture_->getWidth(): %u", bottom_texture_->getWidth());
+    RCLCPP_DEBUG(node_->get_logger(), "image_.rows: %d", image_.rows);
+    RCLCPP_DEBUG(node_->get_logger(), "image_.cols: %d", image_.cols);
+
+    std::vector<cv::Mat> splitted;
+    cv::split(image_, splitted);
+    // Swap channels RGB -> BGR for cv::merge.
+    std::swap(splitted[0], splitted[2]);
+    cv::Mat alpha(image_.rows, image_.cols, CV_8U, cv::Scalar(alpha_ * 255.0));
+    splitted.push_back(alpha);
+    cv::Mat boxMat(image_.rows, image_.cols, CV_8UC4, pDest);
+    cv::merge(splitted, boxMat);
+  } else {
+    memset(pDest, 0, bottom_texture_->getWidth() * bottom_texture_->getHeight());
+    color_.setAlphaF(alpha_);
+    QImage Hud(
+      pDest, bottom_texture_->getWidth(), bottom_texture_->getHeight(), QImage::Format_ARGB32);
+    for (size_t j = 0; j < bottom_texture_->getHeight(); j++) {
+      for (size_t i = 0; i < bottom_texture_->getWidth(); i++) {
+        Hud.setPixel(i, j, color_.rgba());
+      }
+    }
+  }
+  bottom_texture_->getBuffer()->unlock();
+}
+
+// convert sensor_msgs::Image into cv::Mat
+void CameraInfoDisplay::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  cv_bridge::CvImagePtr cv_ptr;
+  try {
+    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
+    image_ = cv_ptr->image;
+    // check the size of bottom texture
+    if (
+      !bottom_texture_ || bottom_texture_->getWidth() != image_.cols ||
+      bottom_texture_->getHeight() != image_.rows) {
+      createTextureForBottom(image_.cols, image_.rows);
+      if (camera_info_) {
+        createCameraInfoShapes(camera_info_);
+      }
+    }
+    image_updated_ = true;
+  } catch (cv_bridge::Exception & e) {
+    RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what());
+  }
+}
+
+void CameraInfoDisplay::createCameraInfoShapes(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg)
+{
+  polygons_.clear();
+  if (edges_) {
+    edges_->clear();
+  }
+  image_geometry::PinholeCameraModel model;
+  bool model_success_p = model.fromCameraInfo(msg);
+  if (!model_success_p) {
+    setStatus(
+      rviz_common::properties::StatusProperty::Error, "Camera Info",
+      "Failed to create camera model from msg");
+    RCLCPP_ERROR(node_->get_logger(), "failed to create camera model");
+    return;
+  }
+  // fx and fy should not be equal 0.
+  if (model.fx() == 0.0 || model.fy() == 0.0) {
+    setStatus(
+      rviz_common::properties::StatusProperty::Error, "Camera Info", "Invalid intrinsic matrix");
+    RCLCPP_ERROR_STREAM(
+      node_->get_logger(),
+      "camera model have invalid intrinsic matrix " << model.intrinsicMatrix());
+    return;
+  }
+  setStatus(rviz_common::properties::StatusProperty::Ok, "Camera Info", "OK");
+
+  ////////////////////////////////////////////////////////
+  // initialize BillboardLine
+  ////////////////////////////////////////////////////////
+  if (!edges_) {
+    edges_.reset(new rviz_rendering::BillboardLine(context_->getSceneManager(), scene_node_));
+    edges_->setLineWidth(0.01);
+  }
+
+  cv::Point2d a(0, 0), b(msg->width, 0), c(msg->width, msg->height), d(0, msg->height);
+  // all the z = 1.0
+  cv::Point3d A = model.projectPixelTo3dRay(a);
+  cv::Point3d B = model.projectPixelTo3dRay(b);
+  cv::Point3d C = model.projectPixelTo3dRay(c);
+  cv::Point3d D = model.projectPixelTo3dRay(d);
+
+  cv::Point3d scaled_A = A * far_clip_distance_;
+  cv::Point3d scaled_B = B * far_clip_distance_;
+  cv::Point3d scaled_C = C * far_clip_distance_;
+  cv::Point3d scaled_D = D * far_clip_distance_;
+
+  cv::Point3d O(0, 0, 0);
+
+  ////////////////////////////////////////////////////////
+  // build polygons
+  ////////////////////////////////////////////////////////
+  if (show_polygons_) {
+    ////////////////////////////////////////////////////////
+    // setup color for polygons
+    ////////////////////////////////////////////////////////
+    Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_);
+    color.a = alpha_;
+    prepareMaterial();
+    if (!not_show_side_polygons_) {
+      material_->getTechnique(0)->getPass(0)->setAmbient(color);
+      {
+        texture_->getBuffer()->lock(Ogre::HardwareBuffer::HBL_NORMAL);
+        const Ogre::PixelBox & pixelBox = texture_->getBuffer()->getCurrentLock();
+        Ogre::uint8 * pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
+        memset(pDest, 0, 1);
+        QImage Hud(pDest, 1, 1, QImage::Format_ARGB32);
+        Hud.setPixel(0, 0, color_.rgba());
+        texture_->getBuffer()->unlock();
+      }
+      addPolygon(O, scaled_B, scaled_A, material_->getName(), true, true);
+      addPolygon(O, scaled_C, scaled_B, material_->getName(), true, true);
+      addPolygon(O, scaled_D, scaled_C, material_->getName(), true, true);
+      addPolygon(O, scaled_A, scaled_D, material_->getName(), true, true);
+    }
+    // bottom
+    drawImageTexture();
+
+    addPolygon(scaled_A, scaled_B, scaled_D, material_bottom_->getName(), false, true);
+    addPolygon(scaled_B, scaled_C, scaled_D, material_bottom_->getName(), false, false);
+  }
+  ////////////////////////////////////////////////////////
+  // build edges
+  ////////////////////////////////////////////////////////
+  if (show_edges_) {
+    edges_->clear();
+    edges_->setMaxPointsPerLine(2);
+    edges_->setNumLines(8);
+    edges_->setColor(
+      edge_color_.red() / 255.0, edge_color_.green() / 255.0, edge_color_.blue() / 255.0, alpha_);
+    addPointToEdge(O);
+    addPointToEdge(scaled_A);
+    edges_->finishLine();
+    addPointToEdge(O);
+    addPointToEdge(scaled_B);
+    edges_->finishLine();
+    addPointToEdge(O);
+    addPointToEdge(scaled_C);
+    edges_->finishLine();
+    addPointToEdge(O);
+    addPointToEdge(scaled_D);
+    edges_->finishLine();
+    addPointToEdge(scaled_A);
+    addPointToEdge(scaled_B);
+    edges_->finishLine();
+    addPointToEdge(scaled_B);
+    addPointToEdge(scaled_C);
+    edges_->finishLine();
+    addPointToEdge(scaled_C);
+    addPointToEdge(scaled_D);
+    edges_->finishLine();
+    addPointToEdge(scaled_D);
+    addPointToEdge(scaled_A);
+  }
+}
+
+////////////////////////////////////////////////////////
+// Properties updating functions
+////////////////////////////////////////////////////////
+void CameraInfoDisplay::updateColor()
+{
+  color_ = color_property_->getColor();
+  if (camera_info_) {
+    createCameraInfoShapes(camera_info_);
+  }
+}
+
+void CameraInfoDisplay::updateEdgeColor()
+{
+  edge_color_ = edge_color_property_->getColor();
+  if (camera_info_) {
+    createCameraInfoShapes(camera_info_);
+  }
+}
+
+void CameraInfoDisplay::updateAlpha()
+{
+  alpha_ = alpha_property_->getFloat();
+  if (camera_info_) {
+    createCameraInfoShapes(camera_info_);
+  }
+}
+
+void CameraInfoDisplay::updateFarClipDistance()
+{
+  far_clip_distance_ = far_clip_distance_property_->getFloat();
+  if (camera_info_) {
+    createCameraInfoShapes(camera_info_);
+  }
+}
+
+void CameraInfoDisplay::updateShowPolygons()
+{
+  show_polygons_ = show_polygons_property_->getBool();
+  if (show_polygons_) {
+    not_show_side_polygons_property_->show();
+  } else {
+    not_show_side_polygons_property_->hide();
+  }
+  if (camera_info_) {
+    createCameraInfoShapes(camera_info_);
+  }
+}
+
+void CameraInfoDisplay::updateShowEdges()
+{
+  show_edges_ = show_edges_property_->getBool();
+  if (camera_info_) {
+    createCameraInfoShapes(camera_info_);
+  }
+}
+
+void CameraInfoDisplay::updateImageTopic()
+{
+  if (use_image_) {
+    std::string topic = image_topic_property_->getStdString();
+    subscribeImage(topic);
+  } else {
+    image_sub_.shutdown();
+    // Set image_updated_ true in order to clear the bottom texture in update() method.
+    image_updated_ = true;
+  }
+}
+
+void CameraInfoDisplay::updateUseImage()
+{
+  use_image_ = use_image_property_->getBool();
+  if (use_image_) {
+    image_topic_property_->show();
+    image_transport_hints_property_->show();
+  } else {
+    image_topic_property_->hide();
+    image_transport_hints_property_->hide();
+  }
+  updateImageTopic();
+}
+void CameraInfoDisplay::updateNotShowSidePolygons()
+{
+  not_show_side_polygons_ = not_show_side_polygons_property_->getBool();
+  if (camera_info_) {
+    createCameraInfoShapes(camera_info_);
+  }
+}
+}  // namespace jsk_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::CameraInfoDisplay, rviz_common::Display)
diff --git a/jsk_rviz_plugins/src/camera_info_display.hpp b/jsk_rviz_plugins/src/camera_info_display.hpp
index 29437ee7..f8a4a18e 100644
--- a/jsk_rviz_plugins/src/camera_info_display.hpp
+++ b/jsk_rviz_plugins/src/camera_info_display.hpp
@@ -28,141 +28,141 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-// #ifndef JSK_RVIZ_PLUGINS_CAMERA_INFO_DISPLAY_H_
-// #define JSK_RVIZ_PLUGINS_CAMERA_INFO_DISPLAY_H_
+// ROS 2 Port:
+// Ander González Tomé <ander.gonzalez@ikerlan.es> @ IKERLAN S. COOP.
 
-// #ifndef Q_MOC_RUN
-// #include <rviz_common/ros_topic_display.hpp>
-// #include <rviz_default_plugins/visibility_control.hpp>
-// #include <rviz_common/ros_topic_display.hpp>
-// #include <rviz_common/properties/color_property.hpp>
-// #include <rviz_common/properties/bool_property.hpp>
-// #include <rviz_common/properties/float_property.hpp>
-// #include <rviz_common/properties/ros_topic_property.hpp>
-// #include <rviz_rendering/objects/shape.hpp>
-// #include <rviz_rendering/objects/billboard_line.hpp>
-// #include <OgreSceneNode.h>
-// //#include <image_geometry/pinhole_camera_model.hpp>
-// #include <sensor_msgs/msg/image.hpp>
-// #include <OgreManualObject.h>
-// #include <OgreSceneManager.h>
-// #include <OgreTextureManager.h>
-// #include <OgreTexture.h>
-// #include <cv_bridge/cv_bridge.h>
-// #include <sensor_msgs/image_encodings.hpp>
-// //#include <image_transport/subscriber.hpp>
+#ifndef JSK_RVIZ_PLUGINS_CAMERA_INFO_DISPLAY_HPP_
+#define JSK_RVIZ_PLUGINS_CAMERA_INFO_DISPLAY_HPP_
 
-// //#include <image_transport_hints_property.hpp>
-// #endif
+#ifndef Q_MOC_RUN
 
-// namespace jsk_rviz_plugins
-// {
-//   class TrianglePolygon
-//   {
-//   public:
-//     typedef std::shared_ptr<TrianglePolygon> Ptr;
-//     TrianglePolygon(Ogre::SceneManager* manager,
-//                     Ogre::SceneNode* node,
-//                     const cv::Point3d& O,
-//                     const cv::Point3d& A,
-//                     const cv::Point3d& B,
-//                     const std::string& name,
-//                     const Ogre::ColourValue& color,
-//                     bool use_color,
-//                     bool upper_triangle);
-//     ~TrianglePolygon();
-//   protected:
-//     Ogre::ManualObject* manual_;
-//     Ogre::SceneManager* manager_;
-//   private:
+#include <OgreManualObject.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreTexture.h>
+#include <OgreTextureManager.h>
+#include <cv_bridge/cv_bridge.hpp>
 
-//   };
+#include <image_transport/subscriber.hpp>
+#include <memory>
+#include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/ros_topic_property.hpp>
+#include <rviz_common/ros_topic_display.hpp>
+#include <rviz_common/visibility_control.hpp>
+#include <rviz_rendering/objects/billboard_line.hpp>
+#include <rviz_rendering/objects/shape.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <string>
 
-//   class CameraInfoDisplay:
-//     public rviz_common::RosTopicDisplay<sensor_msgs::msg::CameraInfo>
-//   {
-//     Q_OBJECT
-//   public:
-//     typedef std::shared_ptr<rviz_rendering::Shape> ShapePtr;
-//     typedef std::shared_ptr<rviz_rendering::BillboardLine> BillboardLinePtr;
-//     CameraInfoDisplay();
-//     ~CameraInfoDisplay();
+#include "image_transport_hints_property.hpp"
+#endif
 
-//   protected:
-//     ////////////////////////////////////////////////////////
-//     // methods required by super class
-//     ////////////////////////////////////////////////////////
-//     void onInitialize() override;
-//     void reset();
-//     void processMessage(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) override;
-//     ////////////////////////////////////////////////////////
-//     // methods
-//     ///////////////////////////////////////////////////////
-//     void update(float wall_dt, float ros_dt) override;
-//     bool isSameCameraInfo(
-//       sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info);
-//     void createCameraInfoShapes(
-//       sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info);
-//     void addPointToEdge(
-//       const cv::Point3d& point);
-//     void addPolygon(
-//       const cv::Point3d& O, const cv::Point3d& A, const cv::Point3d& B, std::string name,
-//       bool use_color, bool upper_triangle);
-//     void prepareMaterial();
-//     void createTextureForBottom(int width, int height);
-//     void imageCallback(const sensor_msgs::Image::ConstSharedPtr& msg);
-//     void drawImageTexture();
-//     //void subscribeImage(std::string topic);
-//     /////////////////////////////////////////////////////////
-//     // variables
-//     ////////////////////////////////////////////////////////
-//     std::vector<TrianglePolygon::Ptr> polygons_;
-//     BillboardLinePtr edges_;
-//     sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_;
-//     Ogre::MaterialPtr material_;
-//     Ogre::TexturePtr texture_;
-//     Ogre::MaterialPtr material_bottom_;
-//     Ogre::TexturePtr bottom_texture_;
-//     image_transport::Subscriber image_sub_;
-//     std::mutex mutex_;
-//     ////////////////////////////////////////////////////////
-//     // variables updated by rviz properties
-//     ////////////////////////////////////////////////////////
-//     double alpha_;
-//     double far_clip_distance_;
-//     QColor color_;
-//     QColor edge_color_;
-//     bool show_polygons_;
-//     bool show_edges_;
-//     bool use_image_;
-//     bool image_updated_;
-//     bool not_show_side_polygons_;
-//     cv::Mat image_;
-//     ////////////////////////////////////////////////////////
-//     // properties
-//     ////////////////////////////////////////////////////////
-//     ImageTransportHintsProperty* image_transport_hints_property_;
-//     rviz_common::properties::FloatProperty* far_clip_distance_property_;
-//     rviz_common::properties::FloatProperty* alpha_property_;
-//     rviz_common::properties::ColorProperty* color_property_;
-//     rviz_common::properties::ColorProperty* edge_color_property_;
-//     rviz_common::properties::BoolProperty* show_polygons_property_;
-//     rviz_common::properties::BoolProperty* not_show_side_polygons_property_;
-//     rviz_common::properties::BoolProperty* use_image_property_;
-//     rviz_common::properties::BoolProperty* show_edges_property_;
+namespace jsk_rviz_plugins
+{
+class TrianglePolygon
+{
+public:
+  typedef std::shared_ptr<TrianglePolygon> Ptr;
+  TrianglePolygon(
+    Ogre::SceneManager * manager, Ogre::SceneNode * node, const cv::Point3d & O,
+    const cv::Point3d & A, const cv::Point3d & B, const std::string & name,
+    const Ogre::ColourValue & color, bool use_color, bool upper_triangle);
+  ~TrianglePolygon();
 
-//   protected Q_SLOTS:
-//     void updateFarClipDistance();
-//     void updateAlpha();
-//     void updateColor();
-//     void updateShowEdges();
-//     void updateShowPolygons();
-//     void updateNotShowSidePolygons();
-//     void updateImageTopic();
-//     void updateUseImage();
-//     void updateEdgeColor();
-//   };
+protected:
+  Ogre::ManualObject * manual_;
+  Ogre::SceneManager * manager_;
 
-// }
+private:
+};
 
-// #endif
+class CameraInfoDisplay : public rviz_common::RosTopicDisplay<sensor_msgs::msg::CameraInfo>
+{
+  Q_OBJECT
+public:
+  typedef std::shared_ptr<rviz_rendering::Shape> ShapePtr;
+  typedef std::shared_ptr<rviz_rendering::BillboardLine> BillboardLinePtr;
+  CameraInfoDisplay();
+  ~CameraInfoDisplay();
+
+protected:
+  ////////////////////////////////////////////////////////
+  // methods required by super class
+  ////////////////////////////////////////////////////////
+  void onInitialize() override;
+  void reset();
+  void processMessage(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) override;
+  ////////////////////////////////////////////////////////
+  // methods
+  ///////////////////////////////////////////////////////
+  void update(float wall_dt, float ros_dt) override;
+  bool isSameCameraInfo(sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info);
+  void createCameraInfoShapes(sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info);
+  void addPointToEdge(const cv::Point3d & point);
+  void addPolygon(
+    const cv::Point3d & O, const cv::Point3d & A, const cv::Point3d & B, std::string name,
+    bool use_color, bool upper_triangle);
+  void prepareMaterial();
+  void createTextureForBottom(int width, int height);
+  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg);
+  void drawImageTexture();
+  void subscribeImage(std::string topic);
+  /////////////////////////////////////////////////////////
+  // variables
+  ////////////////////////////////////////////////////////
+  std::vector<TrianglePolygon::Ptr> polygons_;
+  BillboardLinePtr edges_;
+  sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_;
+  Ogre::MaterialPtr material_;
+  Ogre::TexturePtr texture_;
+  Ogre::MaterialPtr material_bottom_;
+  Ogre::TexturePtr bottom_texture_;
+  image_transport::Subscriber image_sub_;
+  std::mutex mutex_;
+  rclcpp::Node::SharedPtr node_;
+  ////////////////////////////////////////////////////////
+  // variables updated by rviz properties
+  ////////////////////////////////////////////////////////
+  double alpha_;
+  double far_clip_distance_;
+  QColor color_;
+  QColor edge_color_;
+  bool show_polygons_;
+  bool show_edges_;
+  bool use_image_;
+  bool image_updated_;
+  bool not_show_side_polygons_;
+  cv::Mat image_;
+  ////////////////////////////////////////////////////////
+  // properties
+  ////////////////////////////////////////////////////////
+  ImageTransportHintsProperty * image_transport_hints_property_;
+  rviz_common::properties::FloatProperty * far_clip_distance_property_;
+  rviz_common::properties::FloatProperty * alpha_property_;
+  rviz_common::properties::ColorProperty * color_property_;
+  rviz_common::properties::ColorProperty * edge_color_property_;
+  rviz_common::properties::BoolProperty * show_polygons_property_;
+  rviz_common::properties::BoolProperty * not_show_side_polygons_property_;
+  rviz_common::properties::BoolProperty * use_image_property_;
+  rviz_common::properties::RosTopicProperty * image_topic_property_;
+  rviz_common::properties::BoolProperty * show_edges_property_;
+
+protected Q_SLOTS:
+  void updateFarClipDistance();
+  void updateAlpha();
+  void updateColor();
+  void updateShowEdges();
+  void updateShowPolygons();
+  void updateNotShowSidePolygons();
+  void updateImageTopic();
+  void updateUseImage();
+  void updateEdgeColor();
+};
+
+}  // namespace jsk_rviz_plugins
+
+#endif  // JSK_RVIZ_PLUGINS_CAMERA_INFO_DISPLAY_HPP_
diff --git a/jsk_rviz_plugins/src/image_transport_hints_property.cpp b/jsk_rviz_plugins/src/image_transport_hints_property.cpp
new file mode 100644
index 00000000..08ffbd86
--- /dev/null
+++ b/jsk_rviz_plugins/src/image_transport_hints_property.cpp
@@ -0,0 +1,57 @@
+// // -*- mode: c++ -*-
+// Copyright (c) 2014, JSK Lab
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of the JSK Lab nor the names of its
+//    contributors may be used to endorse or promote products derived from
+//    this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+// ROS 2 Port:
+// Ander González Tomé <ander.gonzalez@ikerlan.es> @ IKERLAN S. COOP.
+
+#include "image_transport_hints_property.hpp"
+
+namespace jsk_rviz_plugins
+{
+ImageTransportHintsProperty::~ImageTransportHintsProperty() {}
+
+ImageTransportHintsProperty::ImageTransportHintsProperty(
+  const char * name, const char * description, rviz_common::properties::Property * parent,
+  const char * changed_slot)
+: rviz_common::properties::EditableEnumProperty(name, "raw", description, parent, changed_slot)
+{
+  addOptionStd("raw");
+  addOptionStd("compressed");
+  addOptionStd("theora");
+  setStdString("raw");
+}
+
+image_transport::TransportHints ImageTransportHintsProperty::getTransportHints(
+  const rclcpp::Node * node)
+{
+  return image_transport::TransportHints(node, getStdString());
+}
+
+}  // namespace jsk_rviz_plugins
diff --git a/jsk_rviz_plugins/src/image_transport_hints_property.hpp b/jsk_rviz_plugins/src/image_transport_hints_property.hpp
new file mode 100644
index 00000000..f1009164
--- /dev/null
+++ b/jsk_rviz_plugins/src/image_transport_hints_property.hpp
@@ -0,0 +1,55 @@
+// // -*- mode: c++ -*-
+// Copyright (c) 2014, JSK Lab
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of the JSK Lab nor the names of its
+//    contributors may be used to endorse or promote products derived from
+//    this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+// ROS 2 Port:
+// Ander González Tomé <ander.gonzalez@ikerlan.es> @ IKERLAN S. COOP.
+
+#ifndef JSK_RVIZ_PLUGINS_IMAGE_TRANSPORT_HINTS_PROPERTY_HPP_
+#define JSK_RVIZ_PLUGINS_IMAGE_TRANSPORT_HINTS_PROPERTY_HPP_
+
+#include <image_transport/transport_hints.hpp>
+#include <rviz_common/properties/editable_enum_property.hpp>
+#include <rviz_common/properties/property.hpp>
+
+namespace jsk_rviz_plugins
+{
+class ImageTransportHintsProperty : public rviz_common::properties::EditableEnumProperty
+{
+  Q_OBJECT
+public:
+  ImageTransportHintsProperty(
+    const char * name, const char * description, rviz_common::properties::Property * parent,
+    const char * changed_slot);
+  ~ImageTransportHintsProperty();
+
+  image_transport::TransportHints getTransportHints(const rclcpp::Node * node);
+};
+}  // namespace jsk_rviz_plugins
+#endif  // JSK_RVIZ_PLUGINS_IMAGE_TRANSPORT_HINTS_PROPERTY_HPP_
diff --git a/jsk_rviz_plugins/src/overlay_diagnostic_display.cpp b/jsk_rviz_plugins/src/overlay_diagnostic_display.cpp
index 152fbf22..a90e1d84 100644
--- a/jsk_rviz_plugins/src/overlay_diagnostic_display.cpp
+++ b/jsk_rviz_plugins/src/overlay_diagnostic_display.cpp
@@ -339,7 +339,7 @@ double OverlayDiagnosticDisplay::textWidth(
   painter.setFont(font);
   painter.setPen(pen);
   QFontMetrics metrics(font);
-  const int text_width = metrics.width(text.c_str());
+  const int text_width = metrics.horizontalAdvance(text.c_str());
   const int text_height = metrics.height();
   painter.restore();
   return text_width;
diff --git a/jsk_rviz_plugins/src/overlay_image_display.cpp b/jsk_rviz_plugins/src/overlay_image_display.cpp
index 9eaa8759..a4da0fb9 100644
--- a/jsk_rviz_plugins/src/overlay_image_display.cpp
+++ b/jsk_rviz_plugins/src/overlay_image_display.cpp
@@ -35,7 +35,7 @@
 #include <OgreTechnique.h>
 #include <OgreTexture.h>
 #include <OgreTextureManager.h>
-#include <cv_bridge/cv_bridge.h>
+#include <cv_bridge/cv_bridge.hpp>
 
 #include <rviz_common/uniform_string_stream.hpp>
 #include <sensor_msgs/image_encodings.hpp>
diff --git a/jsk_topic_tools/CMakeLists.txt b/jsk_topic_tools/CMakeLists.txt
index 76a6667d..600f50bb 100644
--- a/jsk_topic_tools/CMakeLists.txt
+++ b/jsk_topic_tools/CMakeLists.txt
@@ -47,11 +47,6 @@ target_include_directories(jsk_topic_tools_node
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
-if (NOT WIN32)
-  ament_environment_hooks(
-    "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}")
-endif()
-
 ament_export_targets(export_jsk_topic_tools_node HAS_LIBRARY_TARGET)
 ament_export_dependencies(rclcpp)
 ament_export_libraries(jsk_topic_tools_node)