ros-perception · SteveMacenski · May 21, 2020 · May 18, 2020 · May 19, 2020 · May 19, 2020
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -1,8 +1,8 @@
 version: 2
 jobs:
-  dashing:
+  foxy:
     docker:
-      - image: ros:dashing-ros-base
+      - image: jwhitleywork/ros:foxy-ros-base-focal
     steps:
       - checkout
       - run:
@@ -13,8 +13,10 @@ jobs:
             apt upgrade -y
             source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
             mkdir -p src/image_pipeline && mv `find -maxdepth 1 -not -name . -not -name src` src/image_pipeline/
+            vcs import src \
+              < src/image_pipeline/tools/ros2_dependencies.repos
             rosdep update
-            rosdep install -y --from-paths . --ignore-src
+            rosdep install -y --ignore-src --from-paths src
       - run:
           name: Build
           command: |
@@ -24,11 +26,11 @@ jobs:
           name: Run Tests
           command: |
             source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
-            colcon test --parallel-workers 1
+            colcon test --packages-select camera_calibration depth_image_proc image_proc image_pipeline image_publisher image_rotate image_view stereo_image_proc --parallel-workers 1
             colcon test-result --verbose
 
 workflows:
   version: 2
   ros_build:
     jobs:
-      - dashing
+      - foxy
diff --git a/depth_image_proc/src/convert_metric.cpp b/depth_image_proc/src/convert_metric.cpp
@@ -88,9 +88,10 @@ void ConvertMetricNode::connectCb()
     sub_raw_.shutdown();
   } else if (!sub_raw_) {
     image_transport::TransportHints hints(this, "raw");
-    sub_raw_ = image_transport::create_subscription(this, "image_raw",
-        std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
-        hints.getTransport());
+    sub_raw_ = image_transport::create_subscription(
+      this, "image_raw",
+      std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
+      hints.getTransport());
   }
 }
 

diff --git a/depth_image_proc/src/crop_foremost.cpp b/depth_image_proc/src/crop_foremost.cpp
@@ -90,9 +90,10 @@ void CropForemostNode::connectCb()
     sub_raw_.shutdown();
   } else if (!sub_raw_) {
     image_transport::TransportHints hints(this, "raw");
-    sub_raw_ = image_transport::create_subscription(this, "image_raw",
-        std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1),
-        hints.getTransport());
+    sub_raw_ = image_transport::create_subscription(
+      this, "image_raw",
+      std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1),
+      hints.getTransport());
   }
 }
 
@@ -108,7 +109,8 @@ void CropForemostNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & r
 
   // Check the number of channels
   if (sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1) {
-    RCLCPP_ERROR(logger_, "Only grayscale image is acceptable, got [%s]",
+    RCLCPP_ERROR(
+      logger_, "Only grayscale image is acceptable, got [%s]",
       raw_msg->encoding.c_str());
     return;
   }

diff --git a/depth_image_proc/src/point_cloud_xyz_radial.cpp b/depth_image_proc/src/point_cloud_xyz_radial.cpp
@@ -113,7 +113,8 @@ cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int heig
 
   if (radial) {
     for (i = 0; i < totalsize; i++) {
-      normalize(pixelVectors.at<cv::Vec3f>(i),
+      normalize(
+        pixelVectors.at<cv::Vec3f>(i),
         dst.at<cv::Vec3f>(i));
     }
     pixelVectors = dst;
@@ -156,8 +157,9 @@ void PointCloudXyzRadialNode::connectCb()
     sub_depth_ = image_transport::create_camera_subscription(
       this,
       "image_raw",
-      std::bind(&PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
-      std::placeholders::_2),
+      std::bind(
+        &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
+        std::placeholders::_2),
       "raw",
       custom_qos);
   }

diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp
@@ -151,7 +151,8 @@ void PointCloudXyziNode::imageCb(
 {
   // Check for bad inputs
   if (depth_msg->header.frame_id != intensity_msg_in->header.frame_id) {
-    RCLCPP_ERROR(logger_, "Depth image frame id [%s] doesn't match image frame id [%s]",
+    RCLCPP_ERROR(
+      logger_, "Depth image frame id [%s] doesn't match image frame id [%s]",
       depth_msg->header.frame_id.c_str(), intensity_msg_in->header.frame_id.c_str());
     return;
   }
@@ -186,7 +187,8 @@ void PointCloudXyziNode::imageCb(
     cv_bridge::CvImage cv_rsz;
     cv_rsz.header = cv_ptr->header;
     cv_rsz.encoding = cv_ptr->encoding;
-    cv::resize(cv_ptr->image.rowRange(0, depth_msg->height / ratio), cv_rsz.image,
+    cv::resize(
+      cv_ptr->image.rowRange(0, depth_msg->height / ratio), cv_rsz.image,
       cv::Size(depth_msg->width, depth_msg->height));
     if ((intensity_msg->encoding == enc::MONO8) || (intensity_msg->encoding == enc::MONO16)) {
       intensity_msg = cv_rsz.toImageMsg();
@@ -206,7 +208,8 @@ void PointCloudXyziNode::imageCb(
     try {
       intensity_msg = cv_bridge::toCvCopy(intensity_msg, enc::MONO8)->toImageMsg();
     } catch (cv_bridge::Exception & e) {
-      RCLCPP_ERROR(logger_, "Unsupported encoding [%s]: %s",
+      RCLCPP_ERROR(
+        logger_, "Unsupported encoding [%s]: %s",
         intensity_msg->encoding.c_str(), e.what());
       return;
     }
@@ -221,7 +224,8 @@ void PointCloudXyziNode::imageCb(
 
   sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
   // pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "i");
-  pcd_modifier.setPointCloud2Fields(4,
+  pcd_modifier.setPointCloud2Fields(
+    4,
     "x", 1, sensor_msgs::msg::PointField::FLOAT32,
     "y", 1, sensor_msgs::msg::PointField::FLOAT32,
     "z", 1, sensor_msgs::msg::PointField::FLOAT32,

diff --git a/depth_image_proc/src/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/point_cloud_xyzi_radial.cpp
@@ -141,7 +141,8 @@ cv::Mat PointCloudXyziRadialNode::initMatrix(
 
   if (radial) {
     for (i = 0; i < totalsize; i++) {
-      normalize(pixelVectors.at<cv::Vec3f>(i),
+      normalize(
+        pixelVectors.at<cv::Vec3f>(i),
         dst.at<cv::Vec3f>(i));
     }
     pixelVectors = dst;
@@ -217,7 +218,8 @@ void PointCloudXyziRadialNode::imageCb(
   cloud_msg->is_bigendian = false;
 
   sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
-  pcd_modifier.setPointCloud2Fields(4,
+  pcd_modifier.setPointCloud2Fields(
+    4,
     "x", 1, sensor_msgs::msg::PointField::FLOAT32,
     "y", 1, sensor_msgs::msg::PointField::FLOAT32,
     "z", 1, sensor_msgs::msg::PointField::FLOAT32,
@@ -249,7 +251,8 @@ void PointCloudXyziRadialNode::imageCb(
   } else if (intensity_msg->encoding == enc::MONO8) {
     convert_intensity<uint8_t>(intensity_msg, cloud_msg);
   } else {
-    RCLCPP_ERROR(logger_, "Intensity image has unsupported encoding [%s]",
+    RCLCPP_ERROR(
+      logger_, "Intensity image has unsupported encoding [%s]",
       intensity_msg->encoding.c_str());
     return;
   }

diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp
@@ -168,7 +168,8 @@ void PointCloudXyzrgbNode::imageCb(
 {
   // Check for bad inputs
   if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id) {
-    RCLCPP_ERROR(logger_, "Depth image frame id [%s] doesn't match RGB image frame id [%s]",
+    RCLCPP_ERROR(
+      logger_, "Depth image frame id [%s] doesn't match RGB image frame id [%s]",
       depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str());
     return;
   }
@@ -203,7 +204,8 @@ void PointCloudXyzrgbNode::imageCb(
     cv_bridge::CvImage cv_rsz;
     cv_rsz.header = cv_ptr->header;
     cv_rsz.encoding = cv_ptr->encoding;
-    cv::resize(cv_ptr->image.rowRange(0, depth_msg->height / ratio), cv_rsz.image,
+    cv::resize(
+      cv_ptr->image.rowRange(0, depth_msg->height / ratio), cv_rsz.image,
       cv::Size(depth_msg->width, depth_msg->height));
     if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) ||
       (rgb_msg->encoding == enc::MONO8))
@@ -213,7 +215,8 @@ void PointCloudXyzrgbNode::imageCb(
       rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg();
     }
 
-    RCLCPP_ERROR(logger_, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)",
+    RCLCPP_ERROR(
+      logger_, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)",
       depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
     return;
   } else {
@@ -261,10 +264,12 @@ void PointCloudXyzrgbNode::imageCb(
   pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
 
   if (depth_msg->encoding == enc::TYPE_16UC1) {
-    convert<uint16_t>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset,
+    convert<uint16_t>(
+      depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset,
       color_step);
   } else if (depth_msg->encoding == enc::TYPE_32FC1) {
-    convert<float>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset,
+    convert<float>(
+      depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset,
       color_step);
   } else {
     RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());

diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp
@@ -157,8 +157,9 @@ void RegisterNode::imageCb(
   Eigen::Affine3d depth_to_rgb;
   try {
     tf2::TimePoint tf2_time(std::chrono::nanoseconds(depth_info_msg->header.stamp.nanosec) +
-      std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::seconds(
-        depth_info_msg->header.stamp.sec)));
+      std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::seconds(
+          depth_info_msg->header.stamp.sec)));
     geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform(
       rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id,
       tf2_time);
@@ -186,7 +187,8 @@ void RegisterNode::imageCb(
   } else if (depth_image_msg->encoding == enc::TYPE_32FC1) {
     convert<float>(depth_image_msg, registered_msg, depth_to_rgb);
   } else {
-    RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]",
+    RCLCPP_ERROR(
+      logger_, "Depth image has unsupported encoding [%s]",
       depth_image_msg->encoding.c_str());
     return;
   }

diff --git a/image_proc/CMakeLists.txt b/image_proc/CMakeLists.txt
@@ -18,7 +18,7 @@ find_package(rcutils REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(OpenCV 3 REQUIRED)
+find_package(OpenCV 4 REQUIRED)
 find_package(image_geometry REQUIRED)
 
 include_directories(

diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp
@@ -46,11 +46,7 @@ void debayer2x2toBGR(
   int R, int G1, int G2, int B)
 {
   typedef cv::Vec<T, 3> DstPixel;  // 8- or 16-bit BGR
-  #if CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION > 2
   dst.create(src.rows / 2, src.cols / 2, cv::traits::Type<DstPixel>::value);
-  #else
-  dst.create(src.rows / 2, src.cols / 2, cv::DataType<DstPixel>::type);
-  #endif
 
   int src_row_step = src.step1();
   int dst_row_step = dst.step1();
@@ -120,8 +116,9 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
 
   pub_ = image_transport::create_camera_publisher(this, "image_raw");
   sub_ = image_transport::create_camera_subscription(
-    this, "image_raw", std::bind(&CropDecimateNode::imageCb, this,
-    std::placeholders::_1, std::placeholders::_2), "raw");
+    this, "image_raw", std::bind(
+      &CropDecimateNode::imageCb, this,
+      std::placeholders::_1, std::placeholders::_2), "raw");
 }
 
 void CropDecimateNode::imageCb(
@@ -152,7 +149,8 @@ void CropDecimateNode::imageCb(
   int max_width = image_msg->width - offset_x_;
 
   if (max_width <= 0) {
-    RCLCPP_WARN(get_logger(),
+    RCLCPP_WARN(
+      get_logger(),
       "x offset is outside the input image width: "
       "%i, x offset: %i.", image_msg->width, offset_x_);
     return;
@@ -161,7 +159,8 @@ void CropDecimateNode::imageCb(
   int max_height = image_msg->height - offset_y_;
 
   if (max_height <= 0) {
-    RCLCPP_WARN(get_logger(),
+    RCLCPP_WARN(
+      get_logger(),
       "y offset is outside the input image height: "
       "%i, y offset: %i", image_msg->height, offset_y_);
     return;
@@ -200,7 +199,8 @@ void CropDecimateNode::imageCb(
   // Special case: when decimating Bayer images, we first do a 2x2 decimation to BGR
   if (is_bayer && (decimation_x > 1 || decimation_y > 1)) {
     if (decimation_x % 2 != 0 || decimation_y % 2 != 0) {
-      RCLCPP_ERROR(get_logger(),
+      RCLCPP_ERROR(
+        get_logger(),
         "Odd decimation not supported for Bayer images");
       return;
     }
@@ -224,7 +224,8 @@ void CropDecimateNode::imageCb(
     } else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16) {
       debayer2x2toBGR<uint16_t>(output.image, bgr, 1, 0, step + 1, step);
     } else {
-      RCLCPP_ERROR(get_logger(), "Unrecognized Bayer encoding '%s'",
+      RCLCPP_ERROR(
+        get_logger(), "Unrecognized Bayer encoding '%s'",
         image_msg->encoding.c_str());
       return;
     }
@@ -271,7 +272,8 @@ void CropDecimateNode::imageCb(
           decimate<16>(output.image, decimated, decimation_x, decimation_y);
           break;
         default:
-          RCLCPP_ERROR(get_logger(),
+          RCLCPP_ERROR(
+            get_logger(),
             "Unsupported pixel size, %d bytes", pixel_size);
           return;
       }

diff --git a/image_proc/src/crop_non_zero.cpp b/image_proc/src/crop_non_zero.cpp
@@ -46,8 +46,10 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
 {
   pub_ = image_transport::create_publisher(this, "image");
   RCLCPP_INFO(this->get_logger(), "subscribe: %s", "image_raw");
-  sub_raw_ = image_transport::create_subscription(this, "image_raw",
-      std::bind(&CropNonZeroNode::imageCb,
+  sub_raw_ = image_transport::create_subscription(
+    this, "image_raw",
+    std::bind(
+      &CropNonZeroNode::imageCb,
       this, std::placeholders::_1), "raw");
 }
 
@@ -63,7 +65,8 @@ void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & ra
 
   // Check the number of channels
   if (sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1) {
-    RCLCPP_ERROR(this->get_logger(), "Only grayscale image is acceptable, got [%s]",
+    RCLCPP_ERROR(
+      this->get_logger(), "Only grayscale image is acceptable, got [%s]",
       raw_msg->encoding.c_str());
     return;
   }
@@ -84,10 +87,11 @@ void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & ra
 
   // search the largest area
   std::vector<std::vector<cv::Point>>::iterator it =
-    std::max_element(cnt.begin(), cnt.end(), [](std::vector<cv::Point> a,
-      std::vector<cv::Point> b) {
-        return a.size() < b.size();
-      });
+    std::max_element(
+    cnt.begin(), cnt.end(), [](std::vector<cv::Point> a,
+    std::vector<cv::Point> b) {
+      return a.size() < b.size();
+    });
 
   cv::Rect r = cv::boundingRect(cnt[std::distance(cnt.begin(), it)]);
 

diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp
@@ -52,8 +52,10 @@ namespace enc = sensor_msgs::image_encodings;
 DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
 : Node("DebayerNode", options)
 {
-  sub_raw_ = image_transport::create_subscription(this, "image_raw",
-      std::bind(&DebayerNode::imageCb, this,
+  sub_raw_ = image_transport::create_subscription(
+    this, "image_raw",
+    std::bind(
+      &DebayerNode::imageCb, this,
       std::placeholders::_1), "raw");
 
   pub_mono_ = image_transport::create_publisher(this, "image_mono");
@@ -143,7 +145,8 @@ void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_ms
     {
       // These algorithms are not in OpenCV yet
       if (raw_msg->encoding != enc::BAYER_GRBG8) {
-        RCLCPP_WARN(this->get_logger(), "Edge aware algorithms currently only support GRBG8 Bayer. "
+        RCLCPP_WARN(
+          this->get_logger(), "Edge aware algorithms currently only support GRBG8 Bayer. "
           "Falling back to bilinear interpolation.");
         algorithm = debayer_bilinear_;
       } else {

diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp
@@ -67,8 +67,9 @@ void RectifyNode::subscribeToCamera()
   {
   */
   sub_camera_ = image_transport::create_camera_subscription(
-    this, "image", std::bind(&RectifyNode::imageCb,
-    this, std::placeholders::_1, std::placeholders::_2), "raw");
+    this, "image", std::bind(
+      &RectifyNode::imageCb,
+      this, std::placeholders::_1, std::placeholders::_2), "raw");
   // }
 }