format calibration_common

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);
   }