Add CI and format code

.clang-format

@@ -3,7 +3,7 @@ Language: Cpp
 BasedOnStyle: Google
 
 AccessModifierOffset: -2
-AlignAfterOpenBracket: AlwaysBreak
+AlignOperands: true
 BraceWrapping:
   AfterClass: true
   AfterFunction: true
@@ -18,3 +18,4 @@ PointerAlignment: Middle
 ReflowComments: false
 SortIncludes: Never
 AllowShortFunctionsOnASingleLine: InlineOnly
+AllowShortLambdasOnASingleLine: All

.github/workflows/ci.yml

@@ -0,0 +1,33 @@
+name: Build and Test
+on:
+  push:
+    branches: [ humble ]
+  pull_request:
+    branches: [ humble ]
+jobs:
+  build-and-test:
+    runs-on: ubuntu-latest
+    container:
+      image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-desktop-latest
+    steps:
+      - name: Check out
+        uses: actions/checkout@v3
+      - name: Pre install
+        run: |
+          /usr/bin/bash -c "apt-get update && apt-get install -y libunwind-dev libceres-dev"
+      - name: Build
+        uses: ros-tooling/[email protected]
+        with:
+          target-ros2-distro: humble
+          skip-tests: true
+      - name: Remove flake8-quotes flake8-import-order for linter
+        run: pip3 uninstall -y flake8-quotes flake8-import-order
+      - name: Test calibration_interfaces
+        run: |
+          /usr/bin/bash .github/workflows/colcon_test.sh calibration_interfaces
+      - name: Test calibration_common
+        run: |
+          /usr/bin/bash .github/workflows/colcon_test.sh calibration_common
+      - name: Test camera_intrinsic_calibration
+        run: |
+          /usr/bin/bash .github/workflows/colcon_test.sh camera_intrinsic_calibration

.github/workflows/colcon_test.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+source /opt/ros/humble/setup.sh
+cd ros_ws
+colcon test --packages-select "$1" --event-handlers console_cohesion+ --return-code-on-test-failure

calibration_common/include/calibration_common/dummy_sensor/dummy_camera_node.hpp

@@ -20,6 +20,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 #include "calibration_common/publisher/image_publisher.hpp"

calibration_common/include/calibration_common/dummy_sensor/dummy_lidar_node.hpp

@@ -20,6 +20,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 #include "calibration_common/publisher/cloud_publisher.hpp"

calibration_common/include/calibration_common/publisher/cloud_publisher.hpp

@@ -34,7 +34,7 @@ class CloudPublisher
     rclcpp::Node::SharedPtr node, std::string topic_name, size_t buffer_size, std::string frame_id);
   ~CloudPublisher() = default;
 
-  template <typename PointT>
+  template<typename PointT>
   void publish(const pcl::PointCloud<PointT> & cloud, rclcpp::Time time)
   {
     sensor_msgs::msg::PointCloud2 msg;
@@ -44,14 +44,14 @@ class CloudPublisher
     publisher_->publish(msg);
   }
 
-  template <typename PointT>
+  template<typename PointT>
   void publish(const pcl::PointCloud<PointT> & cloud, double time)
   {
     rclcpp::Time ros_time(static_cast<uint64_t>(time * 1e9));
     publish(cloud, ros_time);
   }
 
-  template <typename PointT>
+  template<typename PointT>
   void publish(const pcl::PointCloud<PointT> & cloud)
   {
     publish(cloud, node_->get_clock()->now());

calibration_common/include/calibration_common/subscriber/cloud_subscriber.hpp

@@ -28,7 +28,7 @@
 namespace calibration_common
 {
 
-template <typename PointT>
+template<typename PointT>
 class CloudSubscriber
 {
 public:
@@ -41,22 +41,22 @@ class CloudSubscriber
   {
     buffer_size_ = buffer_size;
     auto msg_callback = [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
-      MsgData data;
-      data.time = rclcpp::Time(msg->header.stamp).seconds();
-      data.pointcloud.reset(new pcl::PointCloud<PointT>());
-      pcl::fromROSMsg(*msg, *(data.pointcloud));
-      buffer_mutex_.lock();
-      buffer_.push_back(data);
-      if (buffer_.size() > buffer_size_) {
-        buffer_.pop_front();
-      }
-      buffer_mutex_.unlock();
-    };
+        MsgData data;
+        data.time = rclcpp::Time(msg->header.stamp).seconds();
+        data.pointcloud.reset(new pcl::PointCloud<PointT>());
+        pcl::fromROSMsg(*msg, *(data.pointcloud));
+        buffer_mutex_.lock();
+        buffer_.push_back(data);
+        if (buffer_.size() > buffer_size_) {
+          buffer_.pop_front();
+        }
+        buffer_mutex_.unlock();
+      };
     subscriber_ = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
       topic_name, buffer_size, msg_callback);
   }
 
-  const char * get_topic_name() { return subscriber_->get_topic_name(); }
+  const char * get_topic_name() {return subscriber_->get_topic_name();}
 
   void read(std::deque<MsgData> & output)
   {

calibration_common/launch/dummy_sensors.launch.py

@@ -14,14 +14,11 @@
 
 import os
 
-from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    pkg_calibration_common = get_package_share_directory("calibration_common")
     data_dir = os.path.join(
         os.environ["HOME"], "calibration_data", "SensorsCalibration"
     )

calibration_common/scripts/calibration_client.py

@@ -208,7 +208,7 @@ def destory_calibration_status(self):
         self.initialized = False
 
     def send_command(self, cmd):
-        if self.cmd_pub == None:
+        if self.cmd_pub is None:
             return
         msg = CalibrationCommand()
         msg.command = cmd

calibration_common/src/dummy_sensor/dummy_camera_node.cpp

@@ -49,15 +49,15 @@ DummyCameraNode::DummyCameraNode(const rclcpp::NodeOptions & options)
   // create timer
   auto period_ms = std::chrono::milliseconds(static_cast<int64_t>(1000.0 / rate_));
   auto timer_callback = [this]() {
-    if (loop_ && current_idx_ == images_.size()) {
-      current_idx_ = 0;
-    }
-    if (current_idx_ < images_.size()) {
-      double t = timestamp_ + current_idx_ * 1000.0 / rate_;
-      image_pub_->publish(images_[current_idx_], t);
-      current_idx_++;
-    }
-  };
+      if (loop_ && current_idx_ == images_.size()) {
+        current_idx_ = 0;
+      }
+      if (current_idx_ < images_.size()) {
+        double t = timestamp_ + current_idx_ * 1000.0 / rate_;
+        image_pub_->publish(images_[current_idx_], t);
+        current_idx_++;
+      }
+    };
   timer_ = node_->create_wall_timer(period_ms, timer_callback);
 }
 

calibration_common/src/dummy_sensor/dummy_lidar_node.cpp

@@ -48,18 +48,18 @@ DummyLidarNode::DummyLidarNode(const rclcpp::NodeOptions & options)
   // create timer
   auto period_ms = std::chrono::milliseconds(static_cast<int64_t>(1000.0 / rate_));
   auto timer_callback = [this]() {
-    if (pointclouds_.empty()) {
-      return;
-    }
-    if (loop_ && current_idx_ == pointclouds_.size()) {
-      current_idx_ = 0;
-    }
-    if (current_idx_ < pointclouds_.size()) {
-      double t = timestamp_ + current_idx_ * 1000.0 / rate_;
-      cloud_pub_->publish(*pointclouds_[current_idx_], t);
-      current_idx_++;
-    }
-  };
+      if (pointclouds_.empty()) {
+        return;
+      }
+      if (loop_ && current_idx_ == pointclouds_.size()) {
+        current_idx_ = 0;
+      }
+      if (current_idx_ < pointclouds_.size()) {
+        double t = timestamp_ + current_idx_ * 1000.0 / rate_;
+        cloud_pub_->publish(*pointclouds_[current_idx_], t);
+        current_idx_++;
+      }
+    };
   timer_ = node_->create_wall_timer(period_ms, timer_callback);
 }
 
@@ -72,7 +72,7 @@ bool DummyLidarNode::read_data()
   std::vector<std::string> filepaths;
   for (const auto & entry : std::filesystem::directory_iterator(data_dir_)) {
     if (entry.is_regular_file() && entry.path().extension() == ".pcd") {
-      //std::cout << entry.path() << std::endl; // 输出jpg文件路径
+      // std::cout << entry.path() << std::endl; // 输出jpg文件路径
       filepaths.push_back(entry.path().string());
     }
   }

calibration_common/src/subscriber/image_subscriber.cpp

@@ -29,30 +29,30 @@ ImageSubscriber::ImageSubscriber(
   enable_compressed_ = enable_compressed;
   if (enable_compressed) {
     auto msg_callback = [this](const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
-      MsgData data;
-      data.time = rclcpp::Time(msg->header.stamp).seconds();
-      data.image = cv_bridge::toCvCopy(*msg)->image;
-      buffer_mutex_.lock();
-      buffer_.push_back(data);
-      if (buffer_.size() > buffer_size_) {
-        buffer_.pop_front();
-      }
-      buffer_mutex_.unlock();
-    };
+        MsgData data;
+        data.time = rclcpp::Time(msg->header.stamp).seconds();
+        data.image = cv_bridge::toCvCopy(*msg)->image;
+        buffer_mutex_.lock();
+        buffer_.push_back(data);
+        if (buffer_.size() > buffer_size_) {
+          buffer_.pop_front();
+        }
+        buffer_mutex_.unlock();
+      };
     compressed_subscriber_ = node_->create_subscription<sensor_msgs::msg::CompressedImage>(
       topic_name, buffer_size, msg_callback);
   } else {
     auto msg_callback = [this](const sensor_msgs::msg::Image::SharedPtr msg) {
-      MsgData data;
-      data.time = rclcpp::Time(msg->header.stamp).seconds();
-      data.image = cv_bridge::toCvCopy(*msg)->image;
-      buffer_mutex_.lock();
-      buffer_.push_back(data);
-      if (buffer_.size() > buffer_size_) {
-        buffer_.pop_front();
-      }
-      buffer_mutex_.unlock();
-    };
+        MsgData data;
+        data.time = rclcpp::Time(msg->header.stamp).seconds();
+        data.image = cv_bridge::toCvCopy(*msg)->image;
+        buffer_mutex_.lock();
+        buffer_.push_back(data);
+        if (buffer_.size() > buffer_size_) {
+          buffer_.pop_front();
+        }
+        buffer_mutex_.unlock();
+      };
     subscriber_ =
       node_->create_subscription<sensor_msgs::msg::Image>(topic_name, buffer_size, msg_callback);
   }

camera_intrinsic_calibration/include/camera_intrinsic_calibration/calibration_node.hpp

@@ -17,6 +17,7 @@
 #include <thread>
 #include <memory>
 #include <string>
+#include <deque>
 
 #include "rclcpp/rclcpp.hpp"
 #include "calibration_common/publisher/image_publisher.hpp"

camera_intrinsic_calibration/include/camera_intrinsic_calibration/pinhole_calibrator.hpp

@@ -20,6 +20,7 @@
 #include <deque>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "opencv2/opencv.hpp"
 

camera_intrinsic_calibration/launch/demo.launch.py

@@ -16,7 +16,6 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess
 from launch_ros.actions import Node
 
 

camera_intrinsic_calibration/scripts/calibration_client.py

@@ -236,7 +236,7 @@ def destory_calibration_status(self):
         self.initialized = False
 
     def send_command(self, cmd):
-        if self.cmd_pub == None:
+        if self.cmd_pub is None:
             return
         msg = CalibrationCommand()
         msg.command = cmd

camera_intrinsic_calibration/src/calibration_node.cpp

@@ -76,19 +76,21 @@ CalibrationNode::CalibrationNode(const rclcpp::NodeOptions & options)
   }
   // status timer
   using namespace std::chrono_literals;
-  timer_ = node_->create_wall_timer(100ms, [this]() {
-    std::lock_guard<std::mutex> lock(status_mutex_);
-    status_pub_->publish(status_msg_);
-  });
+  timer_ = node_->create_wall_timer(
+    100ms, [this]() {
+      std::lock_guard<std::mutex> lock(status_mutex_);
+      status_pub_->publish(status_msg_);
+    });
   // thread
-  run_thread_ = std::make_unique<std::thread>([this]() {
-    while (!exit_) {
-      if (!run()) {
-        using namespace std::chrono_literals;
-        std::this_thread::sleep_for(50ms);
+  run_thread_ = std::make_unique<std::thread>(
+    [this]() {
+      while (!exit_) {
+        if (!run()) {
+          using namespace std::chrono_literals;
+          std::this_thread::sleep_for(50ms);
+        }
       }
-    }
-  });
+    });
 }
 
 CalibrationNode::~CalibrationNode()

camera_intrinsic_calibration/src/pinhole_calibrator.cpp

@@ -43,7 +43,7 @@ bool PinholeCalibrator::process_image(const cv::Mat & image)
   std::vector<cv::Point2f> image_corners;
   if (!cv::findChessboardCorners(gray_image, corner_size_, image_corners)) {
     status_message_ = "[collect data] failed to find chessborad corners, total image:" +
-                      std::to_string(total_img_num_);
+      std::to_string(total_img_num_);
     return false;
   }
   cv::TermCriteria criteria(cv::TermCriteria::EPS | cv::TermCriteria::EPS, 50, 0.001);
@@ -60,7 +60,7 @@ bool PinholeCalibrator::process_image(const cv::Mat & image)
   debug_image_ = image;
   cv::drawChessboardCorners(debug_image_, corner_size_, image_corners, true);
   status_message_ = "[collect data] total image:" + std::to_string(total_img_num_) +
-                    ", valid image:" + std::to_string(valid_img_num_) + ".";
+    ", valid image:" + std::to_string(valid_img_num_) + ".";
   return true;
 }
 
@@ -81,7 +81,7 @@ bool PinholeCalibrator::optimize()
     object_points_, image_points_, image_size_, camera_intrinsic_, camera_distortion_,
     board_rotations_, board_translations_);
   status_message_ = "[optimize] successed to optimize, image:" + std::to_string(valid_img_num_) +
-                    ", RMS re-projection error:" + std::to_string(rms);
+    ", RMS re-projection error:" + std::to_string(rms);
   calibreted_ = true;
   return true;
 }