From 0710f23c9e676cc4dbf7b9a0125506ade244574a Mon Sep 17 00:00:00 2001 From: Zhenpeng Ge Date: Wed, 28 Feb 2024 14:59:15 +0000 Subject: [PATCH 1/5] add ci --- .github/workflows/ci.yml | 33 ++++++++++++++++++++++++++++++++ .github/workflows/colcon_test.sh | 4 ++++ 2 files changed, 37 insertions(+) create mode 100644 .github/workflows/ci.yml create mode 100755 .github/workflows/colcon_test.sh diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..ab96188 --- /dev/null +++ b/.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/action-ros-ci@v0.3 + with: + target-ros2-distro: humble + skip-tests: true + - name: Remove flake8-quotes for linter + run: pip3 uninstall -y flake8-quotes + - 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 diff --git a/.github/workflows/colcon_test.sh b/.github/workflows/colcon_test.sh new file mode 100755 index 0000000..66f8e96 --- /dev/null +++ b/.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 From a24b5f2039baeec29bdba468c2977b56803bd396 Mon Sep 17 00:00:00 2001 From: Zhenpeng Ge Date: Wed, 28 Feb 2024 15:46:18 +0000 Subject: [PATCH 2/5] update clang-format --- .clang-format | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.clang-format b/.clang-format index daf4aee..ad6edfa 100644 --- a/.clang-format +++ b/.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 From ecebb15ec405e9bf5f1708ac2e83cb73de2d2342 Mon Sep 17 00:00:00 2001 From: Zhenpeng Ge Date: Wed, 28 Feb 2024 15:52:31 +0000 Subject: [PATCH 3/5] format calibration_common --- .../dummy_sensor/dummy_camera_node.hpp | 1 + .../dummy_sensor/dummy_lidar_node.hpp | 1 + .../publisher/cloud_publisher.hpp | 6 +-- .../subscriber/cloud_subscriber.hpp | 26 ++++++------ .../launch/dummy_sensors.launch.py | 3 -- .../scripts/calibration_client.py | 2 +- .../src/dummy_sensor/dummy_camera_node.cpp | 18 ++++----- .../src/dummy_sensor/dummy_lidar_node.cpp | 26 ++++++------ .../src/subscriber/image_subscriber.cpp | 40 +++++++++---------- 9 files changed, 61 insertions(+), 62 deletions(-) diff --git a/calibration_common/include/calibration_common/dummy_sensor/dummy_camera_node.hpp b/calibration_common/include/calibration_common/dummy_sensor/dummy_camera_node.hpp index 6cb84ce..a75ea70 100644 --- a/calibration_common/include/calibration_common/dummy_sensor/dummy_camera_node.hpp +++ b/calibration_common/include/calibration_common/dummy_sensor/dummy_camera_node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "calibration_common/publisher/image_publisher.hpp" diff --git a/calibration_common/include/calibration_common/dummy_sensor/dummy_lidar_node.hpp b/calibration_common/include/calibration_common/dummy_sensor/dummy_lidar_node.hpp index b7be1f7..4816951 100644 --- a/calibration_common/include/calibration_common/dummy_sensor/dummy_lidar_node.hpp +++ b/calibration_common/include/calibration_common/dummy_sensor/dummy_lidar_node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "calibration_common/publisher/cloud_publisher.hpp" diff --git a/calibration_common/include/calibration_common/publisher/cloud_publisher.hpp b/calibration_common/include/calibration_common/publisher/cloud_publisher.hpp index 1153357..7af27a0 100644 --- a/calibration_common/include/calibration_common/publisher/cloud_publisher.hpp +++ b/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 + template void publish(const pcl::PointCloud & cloud, rclcpp::Time time) { sensor_msgs::msg::PointCloud2 msg; @@ -44,14 +44,14 @@ class CloudPublisher publisher_->publish(msg); } - template + template void publish(const pcl::PointCloud & cloud, double time) { rclcpp::Time ros_time(static_cast(time * 1e9)); publish(cloud, ros_time); } - template + template void publish(const pcl::PointCloud & cloud) { publish(cloud, node_->get_clock()->now()); diff --git a/calibration_common/include/calibration_common/subscriber/cloud_subscriber.hpp b/calibration_common/include/calibration_common/subscriber/cloud_subscriber.hpp index 35e7409..3ac701a 100644 --- a/calibration_common/include/calibration_common/subscriber/cloud_subscriber.hpp +++ b/calibration_common/include/calibration_common/subscriber/cloud_subscriber.hpp @@ -28,7 +28,7 @@ namespace calibration_common { -template +template 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()); - 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()); + 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( 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 & output) { diff --git a/calibration_common/launch/dummy_sensors.launch.py b/calibration_common/launch/dummy_sensors.launch.py index 4c118d5..24f698b 100755 --- a/calibration_common/launch/dummy_sensors.launch.py +++ b/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" ) diff --git a/calibration_common/scripts/calibration_client.py b/calibration_common/scripts/calibration_client.py index aa215c7..1525270 100755 --- a/calibration_common/scripts/calibration_client.py +++ b/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 diff --git a/calibration_common/src/dummy_sensor/dummy_camera_node.cpp b/calibration_common/src/dummy_sensor/dummy_camera_node.cpp index 2dcacad..a856163 100644 --- a/calibration_common/src/dummy_sensor/dummy_camera_node.cpp +++ b/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(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); } diff --git a/calibration_common/src/dummy_sensor/dummy_lidar_node.cpp b/calibration_common/src/dummy_sensor/dummy_lidar_node.cpp index f7411cf..b48f6c0 100644 --- a/calibration_common/src/dummy_sensor/dummy_lidar_node.cpp +++ b/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(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 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()); } } diff --git a/calibration_common/src/subscriber/image_subscriber.cpp b/calibration_common/src/subscriber/image_subscriber.cpp index a6f9cae..453e615 100644 --- a/calibration_common/src/subscriber/image_subscriber.cpp +++ b/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( 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(topic_name, buffer_size, msg_callback); } From 29f23ab2488052370bfc656de458557c0da03dd1 Mon Sep 17 00:00:00 2001 From: Zhenpeng Ge Date: Wed, 28 Feb 2024 16:08:52 +0000 Subject: [PATCH 4/5] update ci file --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ab96188..4684072 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -20,8 +20,8 @@ jobs: with: target-ros2-distro: humble skip-tests: true - - name: Remove flake8-quotes for linter - run: pip3 uninstall -y flake8-quotes + - 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 From 8bde0e21d5df7724bd2c83f90b4ffade9d16e529 Mon Sep 17 00:00:00 2001 From: Zhenpeng Ge Date: Wed, 28 Feb 2024 16:16:46 +0000 Subject: [PATCH 5/5] format camera_intrinsic_calibration --- .../calibration_node.hpp | 1 + .../pinhole_calibrator.hpp | 1 + .../launch/demo.launch.py | 1 - .../scripts/calibration_client.py | 2 +- .../src/calibration_node.cpp | 24 ++++++++++--------- .../src/pinhole_calibrator.cpp | 6 ++--- 6 files changed, 19 insertions(+), 16 deletions(-) diff --git a/camera_intrinsic_calibration/include/camera_intrinsic_calibration/calibration_node.hpp b/camera_intrinsic_calibration/include/camera_intrinsic_calibration/calibration_node.hpp index 479ddeb..20b3828 100644 --- a/camera_intrinsic_calibration/include/camera_intrinsic_calibration/calibration_node.hpp +++ b/camera_intrinsic_calibration/include/camera_intrinsic_calibration/calibration_node.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "calibration_common/publisher/image_publisher.hpp" diff --git a/camera_intrinsic_calibration/include/camera_intrinsic_calibration/pinhole_calibrator.hpp b/camera_intrinsic_calibration/include/camera_intrinsic_calibration/pinhole_calibrator.hpp index 55cfaff..4d9ead9 100644 --- a/camera_intrinsic_calibration/include/camera_intrinsic_calibration/pinhole_calibrator.hpp +++ b/camera_intrinsic_calibration/include/camera_intrinsic_calibration/pinhole_calibrator.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "opencv2/opencv.hpp" diff --git a/camera_intrinsic_calibration/launch/demo.launch.py b/camera_intrinsic_calibration/launch/demo.launch.py index 3b41f8f..0f125df 100755 --- a/camera_intrinsic_calibration/launch/demo.launch.py +++ b/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 diff --git a/camera_intrinsic_calibration/scripts/calibration_client.py b/camera_intrinsic_calibration/scripts/calibration_client.py index 42cac74..e8e58ab 100755 --- a/camera_intrinsic_calibration/scripts/calibration_client.py +++ b/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 diff --git a/camera_intrinsic_calibration/src/calibration_node.cpp b/camera_intrinsic_calibration/src/calibration_node.cpp index 366bca0..168bc42 100644 --- a/camera_intrinsic_calibration/src/calibration_node.cpp +++ b/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 lock(status_mutex_); - status_pub_->publish(status_msg_); - }); + timer_ = node_->create_wall_timer( + 100ms, [this]() { + std::lock_guard lock(status_mutex_); + status_pub_->publish(status_msg_); + }); // thread - run_thread_ = std::make_unique([this]() { - while (!exit_) { - if (!run()) { - using namespace std::chrono_literals; - std::this_thread::sleep_for(50ms); + run_thread_ = std::make_unique( + [this]() { + while (!exit_) { + if (!run()) { + using namespace std::chrono_literals; + std::this_thread::sleep_for(50ms); + } } - } - }); + }); } CalibrationNode::~CalibrationNode() diff --git a/camera_intrinsic_calibration/src/pinhole_calibrator.cpp b/camera_intrinsic_calibration/src/pinhole_calibrator.cpp index 2a1d9e4..e860bf8 100644 --- a/camera_intrinsic_calibration/src/pinhole_calibrator.cpp +++ b/camera_intrinsic_calibration/src/pinhole_calibrator.cpp @@ -43,7 +43,7 @@ bool PinholeCalibrator::process_image(const cv::Mat & image) std::vector 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; }