From ecebb15ec405e9bf5f1708ac2e83cb73de2d2342 Mon Sep 17 00:00:00 2001 From: Zhenpeng Ge Date: Wed, 28 Feb 2024 15:52:31 +0000 Subject: [PATCH] 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); }