Merge branch 'master' into jsk_rviz_plugins_msgs

.github/workflows/config.yml

@@ -50,6 +50,8 @@ jobs:
              sudo mkdir -p /__w/
              sudo chmod 777 -R /__w/
              sudo chown -R $USER $HOME
+             sudo mkdir  -p /__w/_temp/_runner_file_commands/
+             sudo chown -R $USER /__w/_temp/_runner_file_commands/
              # sudo mkdir -p /home/runner/work/_temp/_github_workflow/
              # sudo chown -R $USER $HOME /home/runner/work/_temp/_github_workflow/
              # ls -al /home/runner/work/_temp/_github_workflow/
@@ -58,16 +60,19 @@ jobs:
           fi
 
       - name: Chcekout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v3.0.2
+        with:
+          submodules: true
 
       - name: Cache Download Data
-        uses: actions/cache@v3
+        uses: actions/cache@v3.0.4
         with:
           path: /github/home/.ros/data/jsk_rviz_plugins
           key: jsk_rviz_plugins
 
       - name: Start X server
         run: |
+          if [[ "${{ matrix.CONTAINER }}" =~ "jskrobotics/ros-ubuntu:14.04" ]]; then exit 0; fi
           echo 'debconf debconf/frontend select Noninteractive' | sudo debconf-set-selections || echo "failing on ros-ubuntu is OK" # set non interactive tzdata https://stackoverflow.com/questions/8671308/non-interactive-method-for-dpkg-reconfigure-tzdata
           sudo apt-get -y -qq install mesa-utils x11-xserver-utils xserver-xorg-video-dummy wget
           export DISPLAY=:0
@@ -76,6 +81,21 @@ jobs:
           sleep 3 # wait x server up
           export QT_X11_NO_MITSHM=1 # http://wiki.ros.org/docker/Tutorials/GUI
           xhost +local:root
+        shell: bash
+
+      - name: Start X server (for 14.04)
+        run: |
+          if [[ "${{ matrix.CONTAINER }}" =~ "jskrobotics/ros-ubuntu:14.04" ]]; then
+            echo 'debconf debconf/frontend select Noninteractive' | sudo debconf-set-selections || echo "failing on ros-ubuntu is OK" # set non interactive tzdata https://stackoverflow.com/questions/8671308/non-interactive-method-for-dpkg-reconfigure-tzdata
+            export DISPLAY=:0
+            sudo apt-get install -y xvfb libxkbcommon-x11-0 libxcb-icccm4 libxcb-image0 libxcb-keysyms1 libxcb-randr0 libxcb-render-util0 libxcb-xinerama0 libxcb-xfixes0
+            sudo apt-get install -y x11-xserver-utils  # for xhost
+            sudo /usr/bin/Xvfb $DISPLAY -screen 0 1280x1024x24 &
+            sleep 3 # wait x server up
+            export QT_X11_NO_MITSHM=1 # http://wiki.ros.org/docker/Tutorials/GUI
+            xhost +local:root
+          fi
+        shell: bash
 
       - name: Run jsk_travis
         uses: jsk-ros-pkg/jsk_travis@master

jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/camera_info_publisher.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the Willow Garage nor the names of its
  *     contributors may be used to endorse or promote products derived

jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/footstep_marker.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/pointcloud_cropper.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the Willow Garage nor the names of its
  *     contributors may be used to endorse or promote products derived

jsk_interactive_markers/jsk_interactive_marker/src/bounding_box_marker.cpp

@@ -12,7 +12,7 @@
  *     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/o2r other materials provided
+ *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the Willow Garage nor the names of its
  *     contributors may be used to endorse or promote products derived

jsk_interactive_markers/jsk_interactive_marker/src/camera_info_publisher.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the Willow Garage nor the names of its
  *     contributors may be used to endorse or promote products derived

jsk_interactive_markers/jsk_interactive_marker/src/footstep_marker.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_interactive_markers/jsk_interactive_marker/src/marker_6dof.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the Willow Garage nor the names of its
  *     contributors may be used to endorse or promote products derived

jsk_interactive_markers/jsk_interactive_marker/src/pointcloud_cropper.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the Willow Garage nor the names of its
  *     contributors may be used to endorse or promote products derived

jsk_interactive_markers/jsk_interactive_marker/src/polygon_marker.cpp

@@ -12,7 +12,7 @@
  *     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/o2r other materials provided
+ *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the Willow Garage nor the names of its
  *     contributors may be used to endorse or promote products derived

jsk_rviz_plugins/src/bounding_box_array_display.cpp

@@ -12,7 +12,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/bounding_box_array_display.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/bounding_box_display.cpp

@@ -12,7 +12,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/bounding_box_display.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/bounding_box_display_common.h

@@ -12,7 +12,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/camera_info_display.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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
@@ -41,6 +41,9 @@
 #include <OGRE/OgreBlendMode.h>
 #include <QImage>
 #include <OGRE/OgreHardwarePixelBuffer.h>
+#include <sensor_msgs/image_encodings.h>
+
+namespace enc = sensor_msgs::image_encodings;
 
 namespace jsk_rviz_plugins
 {
@@ -207,7 +210,11 @@ namespace jsk_rviz_plugins
     // move scene_node according to tf
      Ogre::Vector3 position;
      Ogre::Quaternion quaternion;
-     if(!context_->getFrameManager()->getTransform(msg->header.frame_id,
+     std::string frame_id = msg->header.frame_id;
+     if (frame_id[0] == '/') {
+       frame_id = frame_id.substr(1, frame_id.size());
+     }
+     if(!context_->getFrameManager()->getTransform(frame_id,
                                                    msg->header.stamp,
                                                    position,
                                                    quaternion)) {
@@ -240,7 +247,11 @@ namespace jsk_rviz_plugins
         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;
+        msg->distortion_model == camera_info_->distortion_model &&
+        msg->roi.x_offset == camera_info_->roi.x_offset &&
+        msg->roi.y_offset == camera_info_->roi.y_offset &&
+        msg->roi.height == camera_info_->roi.height &&
+        msg->roi.width == camera_info_->roi.width;
       if (meta_same_p) {
         for (size_t i = 0; i < msg->P.size(); i++) {
           if (msg->P[i] != camera_info_->P[i]) {
@@ -404,11 +415,67 @@ namespace jsk_rviz_plugins
       const sensor_msgs::Image::ConstPtr& msg)
   {
     boost::mutex::scoped_lock lock(mutex_);
-    cv_bridge::CvImagePtr cv_ptr;
+    cv_bridge::CvImageConstPtr cv_ptr;
+    if (!camera_info_) {
+      return;
+    }
     try
     {
-      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
-      image_ = cv_ptr->image;
+      cv_ptr = cv_bridge::toCvShare(msg);
+      cv::Mat im = cv_ptr->image.clone();
+      if (msg->encoding == enc::BGRA8) {
+        cv::cvtColor(im, im, cv::COLOR_BGRA2RGB);
+      } else if (msg->encoding == enc::BGRA16) {
+        im.convertTo(im, CV_8U, 1 / 256.0);
+        cv::cvtColor(im, im, cv::COLOR_BGRA2RGB);
+      } else if (msg->encoding == enc::BGR8) {
+        cv::cvtColor(im, im, cv::COLOR_BGR2RGB);
+      } else if (msg->encoding == enc::BGR16) {
+        im.convertTo(im, CV_8U, 1 / 256.0);
+        cv::cvtColor(im, im, cv::COLOR_BGR2RGB);
+      } else if (msg->encoding == enc::RGBA8) {
+        cv::cvtColor(im, im, cv::COLOR_RGBA2RGB);
+      } else if (msg->encoding == enc::RGBA16) {
+        im.convertTo(im, CV_8U, 1 / 256.0);
+        cv::cvtColor(im, im, cv::COLOR_RGBA2RGB);
+      } else if (msg->encoding == enc::RGB8) {
+        // nothing
+      } else if (msg->encoding == enc::RGB16) {
+        im.convertTo(im, CV_8U, 1 / 256.0);
+      } else if (msg->encoding == enc::MONO8) {
+        cv::cvtColor(im, im, cv::COLOR_GRAY2RGB);
+      } else if (msg->encoding == enc::MONO16) {
+        im.convertTo(im, CV_8U, 1 / 256.0);
+        cv::cvtColor(im, im, cv::COLOR_GRAY2RGB);
+      } else {
+        ROS_ERROR("[CameraInfoDisplay] Not supported image encodings %s.", msg->encoding.c_str());
+        return;
+      }
+
+      int roi_height = camera_info_->roi.height ? camera_info_->roi.height : camera_info_->height;
+      int roi_width = camera_info_->roi.width ? camera_info_->roi.width : camera_info_->width;
+      if (camera_info_->binning_y > 0) {
+        roi_height /= camera_info_->binning_y;
+      }
+      if (camera_info_->binning_x > 0) {
+        roi_width /= camera_info_->binning_x;
+      }
+
+      if (im.cols == camera_info_->width && im.rows == camera_info_->height) {
+        cv::Rect roi(camera_info_->roi.x_offset, camera_info_->roi.y_offset,
+                     camera_info_->roi.width ? camera_info_->roi.width : camera_info_->width,
+                     camera_info_->roi.height ? camera_info_->roi.height : camera_info_->height);
+        image_ = cv::Mat(im, roi).clone();
+      } else if (im.cols == roi_width && im.rows == roi_height) {
+        image_ = im.clone();
+      } else {
+        ROS_ERROR("[CameraInfoDisplay] Invalid image size (w, h) = (%d, %d), expected (w, h) = (%d, %d) or (%d, %d) (ROI size)",
+                  im.cols, im.rows,
+                  camera_info_->width, camera_info_->height,
+                  roi_width, roi_height);
+        return;
+      }
+
       // check the size of bottom texture
       if (bottom_texture_.isNull()
           || bottom_texture_->getWidth() != image_.cols
@@ -457,8 +524,17 @@ namespace jsk_rviz_plugins
       edges_->setLineWidth(0.01);
     }
 
-    cv::Point2d a(0, 0), b(msg->width, 0),
-      c(msg->width, msg->height), d(0, msg->height);
+    int height = msg->roi.height ? msg->roi.height : msg->height;
+    int width = msg->roi.width ? msg->roi.width : msg->width;
+    if (msg->binning_y > 0) {
+      height /= msg->binning_y;
+    }
+    if (msg->binning_x > 0) {
+      width /= msg->binning_x;
+    }
+
+    cv::Point2d a(0, 0), b(width, 0),
+      c(width, height), d(0, height);
     // all the z = 1.0
     cv::Point3d A = model.projectPixelTo3dRay(a);
     cv::Point3d B = model.projectPixelTo3dRay(b);

jsk_rviz_plugins/src/camera_info_display.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/close_all_tool.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/close_all_tool.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/diagnostics_display.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/diagnostics_display.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/facing_visualizer.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/facing_visualizer.h

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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

jsk_rviz_plugins/src/footstep_display.cpp

@@ -13,7 +13,7 @@
  *     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/o2r other materials provided
+ *     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