#include "../include/include/realtime_utils/realtime_utils.hpp"
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>
#include <chrono>
#include <thread>
#include <vector>
#include <string>
#include <memory>
#include <atomic>
#include <limits>
using namespace std::chrono_literals;
// 原子操作辅助工具
namespace atomic_utils {
template<typename T>
void atomic_update_max(std::atomic<T>& atomic_var, T value) {
T current = atomic_var.load();
while (value > current &&
!atomic_var.compare_exchange_weak(current, value)) {}
}
template<typename T>
void atomic_update_min(std::atomic<T>& atomic_var, T value) {
T current = atomic_var.load();
while (value < current &&
!atomic_var.compare_exchange_weak(current, value)) {}
}
}
// 多线程订阅者节点
class MultiThreadSubscriber : public rclcpp::Node {
public:
// 线程统计结构(全原子操作)
struct ThreadStats {
std::atomic<uint64_t> message_count{0};
std::atomic<uint64_t> total_latency{0}; // 改为uint64避免溢出
std::atomic<int64_t> max_latency{0};
std::atomic<int64_t> min_latency{std::numeric_limits<int64_t>::max()};
// 原子重置方法
void reset() {
message_count = 0;
total_latency = 0;
max_latency = 0;
min_latency = std::numeric_limits<int64_t>::max();
}
};
// 消息数据结构
struct MessageData {
double value;
std::chrono::steady_clock::time_point receive_time;
int thread_id;
};
// 构造函数
MultiThreadSubscriber() : Node("multithread_subscriber") {
// 参数声明
declare_parameter("thread_count", 4);
declare_parameter("process_time_us", 200);
// 获取参数
int thread_count = get_parameter("thread_count").as_int();
int process_time = get_parameter("process_time_us").as_int();
thread_stats_.resize(thread_count);
// 创建订阅线程
for (int i = 0; i < thread_count; ++i) {
threads_.emplace_back(&MultiThreadSubscriber::subscriber_loop, this, i, process_time);
}
// 创建低优先级监控线程
monitor_thread_ = std::thread([this]() {
pthread_setname_np(pthread_self(), "monitor_thread");
realtime_utils::RTThread::set_current_thread_priority(SCHED_OTHER, 0);
monitor_loop();
});
}
// 析构函数
~MultiThreadSubscriber() {
running_ = false;
for (auto& thread : threads_) {
if (thread.joinable()) thread.join();
}
if (monitor_thread_.joinable()) monitor_thread_.join();
}
private:
// 订阅线程主循环
void subscriber_loop(int thread_id, int process_time) {
// 设置高实时优先级
std::string thread_name = "sub_thread_" + std::to_string(thread_id);
pthread_setname_np(pthread_self(), thread_name.c_str());
if (!realtime_utils::RTThread::set_current_thread_priority(
SCHED_FIFO, 90 - thread_id)) { // 提高优先级
RCLCPP_ERROR(get_logger(), "Thread %d failed to set real-time priority", thread_id);
}
// 创建专属话题
auto topic_name = "control_topic_" + std::to_string(thread_id);
auto qos = rclcpp::QoS(100).reliable().durability_volatile(); // 增加队列大小
// 创建订阅器
auto sub = create_subscription<std_msgs::msg::Float64>(
topic_name, qos,
[this, thread_id](const std_msgs::msg::Float64::SharedPtr msg) {
MessageData data;
data.value = msg->data;
data.receive_time = std::chrono::steady_clock::now();
data.thread_id = thread_id;
if (!message_buffer_.push(data)) {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
"Thread %d buffer full, message dropped", thread_id);
}
});
// 主处理循环
while (rclcpp::ok() && running_) {
MessageData data;
if (message_buffer_.pop(data)) {
// 模拟处理耗时
std::this_thread::sleep_for(std::chrono::microseconds(process_time));
// 计算端到端延迟
auto latency = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::steady_clock::now() - data.receive_time).count();
// 无锁更新统计
update_thread_stats(data.thread_id, latency);
} else {
std::this_thread::sleep_for(100us); // 更短的休眠
}
}
}
// 无锁统计更新
void update_thread_stats(size_t thread_id, int64_t latency) {
if (thread_id >= thread_stats_.size()) return;
auto& stats = thread_stats_[thread_id];
stats.message_count++;
stats.total_latency += latency;
atomic_utils::atomic_update_max(stats.max_latency, latency);
atomic_utils::atomic_update_min(stats.min_latency, latency);
}
// 监控循环(无锁)
void monitor_loop() {
auto last_print = std::chrono::steady_clock::now();
while (rclcpp::ok() && running_) {
std::this_thread::sleep_for(5s); // 降低输出频率
// 原子获取统计快照
std::vector<ThreadStats> snapshot;
for (auto& stats : thread_stats_) {
ThreadStats s;
s.message_count = stats.message_count.exchange(0);
s.total_latency = stats.total_latency.exchange(0);
s.max_latency = stats.max_latency.exchange(0);
s.min_latency = stats.min_latency.exchange(std::numeric_limits<int64_t>::max());
snapshot.push_back(s);
}
// 输出统计(非实时上下文)
print_stats(snapshot);
}
}
// 打印统计(无锁)
void print_stats(const std::vector<ThreadStats>& snapshot) {
RCLCPP_INFO(get_logger(), "\n=== Subscriber Threads Stats ===");
for (size_t i = 0; i < snapshot.size(); ++i) {
const auto& stats = snapshot[i];
if (stats.message_count == 0) continue;
double avg_latency = static_cast<double>(stats.total_latency) / stats.message_count;
RCLCPP_INFO(get_logger(),
"Thread %zu: %lu msgs | Avg: %.2f us | Max: %ld us | Min: %ld us",
i, stats.message_count.load(), avg_latency,
stats.max_latency.load(), stats.min_latency.load());
}
}
// 成员变量
std::vector<std::thread> threads_;
std::thread monitor_thread_;
std::atomic<bool> running_{true};
realtime_utils::LockFreeRingBuffer<MessageData, 20000> message_buffer_; // 增大缓冲区
std::vector<ThreadStats> thread_stats_; // 无锁统计
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<MultiThreadSubscriber>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
#include "../include/include/realtime_utils/realtime_utils.hpp"
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>
#include <chrono>
#include <thread>
#include <vector>
#include <string>
#include <memory>
#include <atomic>
#include <limits>
using namespace std::chrono_literals;
// 原子操作辅助工具
namespace atomic_utils {
template<typename T>
void atomic_update_max(std::atomic<T>& atomic_var, T value) {
T current = atomic_var.load();
while (value > current &&
!atomic_var.compare_exchange_weak(current, value)) {}
}
template<typename T>
void atomic_update_min(std::atomic<T>& atomic_var, T value) {
T current = atomic_var.load();
while (value < current &&
!atomic_var.compare_exchange_weak(current, value)) {}
}
}
// 多线程发布者节点
class MultiThreadPublisher : public rclcpp::Node {
public:
// 线程统计结构(全原子操作)
struct ThreadStats {
std::atomic<uint64_t> message_count{0};
std::atomic<uint64_t> total_latency{0}; // 改为uint64避免溢出
std::atomic<int64_t> max_latency{0};
std::atomic<int64_t> min_latency{std::numeric_limits<int64_t>::max()};
// 原子重置方法
void reset() {
message_count = 0;
total_latency = 0;
max_latency = 0;
min_latency = std::numeric_limits<int64_t>::max();
}
};
// 构造函数
MultiThreadPublisher() : Node("multithread_publisher") {
// 参数声明
declare_parameter("thread_count", 4);
declare_parameter("frequency", 500.0);
// 获取参数
int thread_count = get_parameter("thread_count").as_int();
double frequency = get_parameter("frequency").as_double();
thread_stats_.resize(thread_count); // 初始化统计容器
// 创建发布线程
for (int i = 0; i < thread_count; ++i) {
threads_.emplace_back(&MultiThreadPublisher::publisher_loop, this,
i, frequency);
}
// 创建低优先级监控线程
monitor_thread_ = std::thread([this]() {
pthread_setname_np(pthread_self(), "monitor_thread");
realtime_utils::RTThread::set_current_thread_priority(SCHED_OTHER, 0);
monitor_loop();
});
}
// 析构函数
~MultiThreadPublisher() {
running_ = false;
for (auto& thread : threads_) {
if (thread.joinable()) thread.join();
}
if (monitor_thread_.joinable()) monitor_thread_.join();
}
private:
// 发布线程主循环
void publisher_loop(int thread_id, double frequency) {
// 设置线程名和实时优先级
std::string thread_name = "pub_thread_" + std::to_string(thread_id);
pthread_setname_np(pthread_self(), thread_name.c_str());
if (!realtime_utils::RTThread::set_current_thread_priority(
SCHED_FIFO, 90 - thread_id)) { // 提高优先级
RCLCPP_ERROR(get_logger(), "Thread %d failed to set real-time priority", thread_id);
}
// 创建专属话题和发布器
auto topic_name = "control_topic_" + std::to_string(thread_id);
auto qos = rclcpp::QoS(100).reliable().durability_volatile(); // 增加队列大小
auto pub = create_publisher<std_msgs::msg::Float64>(topic_name, qos);
// 计算发布周期
const auto period = std::chrono::nanoseconds(static_cast<int64_t>(1e9 / frequency));
auto start_time = std::chrono::steady_clock::now();
uint64_t count = 0;
// 复用消息对象
auto msg = std::make_shared<std_msgs::msg::Float64>();
msg->data = thread_id;
while (rclcpp::ok() && running_) {
auto next_time = start_time + count * period;
std::this_thread::sleep_until(next_time);
msg->data = count;
pub->publish(*msg);
auto latency = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::steady_clock::now() - next_time).count();
// 无锁更新统计
update_thread_stats(thread_id, latency);
count++;
}
}
// 无锁统计更新
void update_thread_stats(size_t thread_id, int64_t latency) {
if (thread_id >= thread_stats_.size()) return;
auto& stats = thread_stats_[thread_id];
stats.message_count++;
stats.total_latency += latency;
atomic_utils::atomic_update_max(stats.max_latency, latency);
atomic_utils::atomic_update_min(stats.min_latency, latency);
}
// 监控循环(无锁)
void monitor_loop() {
auto last_print = std::chrono::steady_clock::now();
while (rclcpp::ok() && running_) {
std::this_thread::sleep_for(5s); // 降低输出频率
// 原子获取统计快照
std::vector<ThreadStats> snapshot;
for (auto& stats : thread_stats_) {
ThreadStats s;
s.message_count = stats.message_count.exchange(0);
s.total_latency = stats.total_latency.exchange(0);
s.max_latency = stats.max_latency.exchange(0);
s.min_latency = stats.min_latency.exchange(std::numeric_limits<int64_t>::max());
snapshot.push_back(s);
}
// 输出统计(非实时上下文)
print_stats(snapshot);
}
}
// 打印统计(无锁)
void print_stats(const std::vector<ThreadStats>& snapshot) {
RCLCPP_INFO(get_logger(), "\n=== Publisher Threads Stats ===");
for (size_t i = 0; i < snapshot.size(); ++i) {
const auto& stats = snapshot[i];
if (stats.message_count == 0) continue;
double avg_latency = static_cast<double>(stats.total_latency) / stats.message_count;
RCLCPP_INFO(get_logger(),
"Thread %zu: %lu msgs | Avg: %.2f us | Max: %ld us | Min: %ld us",
i, stats.message_count.load(), avg_latency,
stats.max_latency.load(), stats.min_latency.load());
}
}
// 成员变量
std::vector<std::thread> threads_;
std::thread monitor_thread_;
std::atomic<bool> running_{true};
std::vector<ThreadStats> thread_stats_; // 无锁统计
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
if (getuid() != 0) {
RCLCPP_ERROR(rclcpp::get_logger("main"),
"This program requires root privileges for real-time scheduling");
return 1;
}
auto node = std::make_shared<MultiThreadPublisher>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
报错:
--- stderr: realtime_demo
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp: In member function ‘void MultiThreadSubscriber::monitor_loop()’:
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:154:14: warning: variable ‘last_print’ set but not used [-Wunused-but-set-variable]
154 | auto last_print = std::chrono::steady_clock::now();
| ^~~~~~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp: In member function ‘void MultiThreadPublisher::monitor_loop()’:
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:140:14: warning: variable ‘last_print’ set but not used [-Wunused-but-set-variable]
140 | auto last_print = std::chrono::steady_clock::now();
| ^~~~~~~~~~
In file included from /usr/include/x86_64-linux-gnu/c++/11/bits/c++allocator.h:33,
from /usr/include/c++/11/bits/allocator.h:46,
from /usr/include/c++/11/memory:64,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:153,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:1:
/usr/include/c++/11/ext/new_allocator.h: In instantiation of ‘void __gnu_cxx::new_allocator<_Tp>::construct(_Up*, _Args&& ...) [with _Up = MultiThreadSubscriber::ThreadStats; _Args = {const MultiThreadSubscriber::ThreadStats&}; _Tp = MultiThreadSubscriber::ThreadStats]’:
/usr/include/c++/11/bits/alloc_traits.h:516:17: required from ‘static void std::allocator_traits<std::allocator<_Tp1> >::construct(std::allocator_traits<std::allocator<_Tp1> >::allocator_type&, _Up*, _Args&& ...) [with _Up = MultiThreadSubscriber::ThreadStats; _Args = {const MultiThreadSubscriber::ThreadStats&}; _Tp = MultiThreadSubscriber::ThreadStats; std::allocator_traits<std::allocator<_Tp1> >::allocator_type = std::allocator<MultiThreadSubscriber::ThreadStats>]’
/usr/include/c++/11/bits/stl_vector.h:1192:30: required from ‘void std::vector<_Tp, _Alloc>::push_back(const value_type&) [with _Tp = MultiThreadSubscriber::ThreadStats; _Alloc = std::allocator<MultiThreadSubscriber::ThreadStats>; std::vector<_Tp, _Alloc>::value_type = MultiThreadSubscriber::ThreadStats]’
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:167:35: required from here
/usr/include/c++/11/ext/new_allocator.h:162:11: error: use of deleted function ‘MultiThreadSubscriber::ThreadStats::ThreadStats(const MultiThreadSubscriber::ThreadStats&)’
162 | { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); }
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:35:12: note: ‘MultiThreadSubscriber::ThreadStats::ThreadStats(const MultiThreadSubscriber::ThreadStats&)’ is implicitly deleted because the default definition would be ill-formed:
35 | struct ThreadStats {
| ^~~~~~~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:35:12: error: use of deleted function ‘std::atomic<long unsigned int>::atomic(const std::atomic<long unsigned int>&)’
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:1:
/usr/include/c++/11/atomic:898:7: note: declared here
898 | atomic(const atomic&) = delete;
| ^~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:35:12: error: use of deleted function ‘std::atomic<long unsigned int>::atomic(const std::atomic<long unsigned int>&)’
35 | struct ThreadStats {
| ^~~~~~~~~~~
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:1:
/usr/include/c++/11/atomic:898:7: note: declared here
898 | atomic(const atomic&) = delete;
| ^~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:35:12: error: use of deleted function ‘std::atomic<long int>::atomic(const std::atomic<long int>&)’
35 | struct ThreadStats {
| ^~~~~~~~~~~
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:1:
/usr/include/c++/11/atomic:875:7: note: declared here
875 | atomic(const atomic&) = delete;
| ^~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:35:12: error: use of deleted function ‘std::atomic<long int>::atomic(const std::atomic<long int>&)’
35 | struct ThreadStats {
| ^~~~~~~~~~~
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:1:
/usr/include/c++/11/atomic:875:7: note: declared here
875 | atomic(const atomic&) = delete;
| ^~~~~~
In file included from /usr/include/x86_64-linux-gnu/c++/11/bits/c++allocator.h:33,
from /usr/include/c++/11/bits/allocator.h:46,
from /usr/include/c++/11/memory:64,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:153,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:1:
/usr/include/c++/11/ext/new_allocator.h: In instantiation of ‘void __gnu_cxx::new_allocator<_Tp>::construct(_Up*, _Args&& ...) [with _Up = MultiThreadPublisher::ThreadStats; _Args = {const MultiThreadPublisher::ThreadStats&}; _Tp = MultiThreadPublisher::ThreadStats]’:
/usr/include/c++/11/bits/alloc_traits.h:516:17: required from ‘static void std::allocator_traits<std::allocator<_Tp1> >::construct(std::allocator_traits<std::allocator<_Tp1> >::allocator_type&, _Up*, _Args&& ...) [with _Up = MultiThreadPublisher::ThreadStats; _Args = {const MultiThreadPublisher::ThreadStats&}; _Tp = MultiThreadPublisher::ThreadStats; std::allocator_traits<std::allocator<_Tp1> >::allocator_type = std::allocator<MultiThreadPublisher::ThreadStats>]’
/usr/include/c++/11/bits/stl_vector.h:1192:30: required from ‘void std::vector<_Tp, _Alloc>::push_back(const value_type&) [with _Tp = MultiThreadPublisher::ThreadStats; _Alloc = std::allocator<MultiThreadPublisher::ThreadStats>; std::vector<_Tp, _Alloc>::value_type = MultiThreadPublisher::ThreadStats]’
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:153:35: required from here
/usr/include/c++/11/ext/new_allocator.h:162:11: error: use of deleted function ‘MultiThreadPublisher::ThreadStats::ThreadStats(const MultiThreadPublisher::ThreadStats&)’
162 | { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); }
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:35:12: note: ‘MultiThreadPublisher::ThreadStats::ThreadStats(const MultiThreadPublisher::ThreadStats&)’ is implicitly deleted because the default definition would be ill-formed:
35 | struct ThreadStats {
| ^~~~~~~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:35:12: error: use of deleted function ‘std::atomic<long unsigned int>::atomic(const std::atomic<long unsigned int>&)’
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:1:
/usr/include/c++/11/atomic:898:7: note: declared here
898 | atomic(const atomic&) = delete;
| ^~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:35:12: error: use of deleted function ‘std::atomic<long unsigned int>::atomic(const std::atomic<long unsigned int>&)’
35 | struct ThreadStats {
| ^~~~~~~~~~~
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:1:
/usr/include/c++/11/atomic:898:7: note: declared here
898 | atomic(const atomic&) = delete;
| ^~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:35:12: error: use of deleted function ‘std::atomic<long int>::atomic(const std::atomic<long int>&)’
35 | struct ThreadStats {
| ^~~~~~~~~~~
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:1:
/usr/include/c++/11/atomic:875:7: note: declared here
875 | atomic(const atomic&) = delete;
| ^~~~~~
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:35:12: error: use of deleted function ‘std::atomic<long int>::atomic(const std::atomic<long int>&)’
35 | struct ThreadStats {
| ^~~~~~~~~~~
In file included from /usr/include/c++/11/future:41,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/executors.hpp:18,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:155,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:1:
/usr/include/c++/11/atomic:875:7: note: declared here
875 | atomic(const atomic&) = delete;
| ^~~~~~
In file included from /usr/include/c++/11/memory:66,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:153,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:1:
/usr/include/c++/11/bits/stl_uninitialized.h: In instantiation of ‘_ForwardIterator std::uninitialized_copy(_InputIterator, _InputIterator, _ForwardIterator) [with _InputIterator = std::move_iterator<MultiThreadPublisher::ThreadStats*>; _ForwardIterator = MultiThreadPublisher::ThreadStats*]’:
/usr/include/c++/11/bits/stl_uninitialized.h:333:37: required from ‘_ForwardIterator std::__uninitialized_copy_a(_InputIterator, _InputIterator, _ForwardIterator, std::allocator<_Tp>&) [with _InputIterator = std::move_iterator<MultiThreadPublisher::ThreadStats*>; _ForwardIterator = MultiThreadPublisher::ThreadStats*; _Tp = MultiThreadPublisher::ThreadStats]’
/usr/include/c++/11/bits/stl_uninitialized.h:355:2: required from ‘_ForwardIterator std::__uninitialized_move_if_noexcept_a(_InputIterator, _InputIterator, _ForwardIterator, _Allocator&) [with _InputIterator = MultiThreadPublisher::ThreadStats*; _ForwardIterator = MultiThreadPublisher::ThreadStats*; _Allocator = std::allocator<MultiThreadPublisher::ThreadStats>]’
/usr/include/c++/11/bits/vector.tcc:659:48: required from ‘void std::vector<_Tp, _Alloc>::_M_default_append(std::vector<_Tp, _Alloc>::size_type) [with _Tp = MultiThreadPublisher::ThreadStats; _Alloc = std::allocator<MultiThreadPublisher::ThreadStats>; std::vector<_Tp, _Alloc>::size_type = long unsigned int]’
/usr/include/c++/11/bits/stl_vector.h:940:4: required from ‘void std::vector<_Tp, _Alloc>::resize(std::vector<_Tp, _Alloc>::size_type) [with _Tp = MultiThreadPublisher::ThreadStats; _Alloc = std::allocator<MultiThreadPublisher::ThreadStats>; std::vector<_Tp, _Alloc>::size_type = long unsigned int]’
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_publisher.cpp:60:29: required from here
/usr/include/c++/11/bits/stl_uninitialized.h:138:72: error: static assertion failed: result type must be constructible from value type of input range
138 | static_assert(is_constructible<_ValueType2, decltype(*__first)>::value,
| ^~~~~
/usr/include/c++/11/bits/stl_uninitialized.h:138:72: note: ‘std::integral_constant<bool, false>::value’ evaluates to false
In file included from /usr/include/c++/11/memory:66,
from /home/yidds/YiDDS/YiDDS_humble/install/rclcpp/include/rclcpp/rclcpp/rclcpp.hpp:153,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/../include/include/realtime_utils/realtime_utils.hpp:3,
from /home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:1:
/usr/include/c++/11/bits/stl_uninitialized.h: In instantiation of ‘_ForwardIterator std::uninitialized_copy(_InputIterator, _InputIterator, _ForwardIterator) [with _InputIterator = std::move_iterator<MultiThreadSubscriber::ThreadStats*>; _ForwardIterator = MultiThreadSubscriber::ThreadStats*]’:
/usr/include/c++/11/bits/stl_uninitialized.h:333:37: required from ‘_ForwardIterator std::__uninitialized_copy_a(_InputIterator, _InputIterator, _ForwardIterator, std::allocator<_Tp>&) [with _InputIterator = std::move_iterator<MultiThreadSubscriber::ThreadStats*>; _ForwardIterator = MultiThreadSubscriber::ThreadStats*; _Tp = MultiThreadSubscriber::ThreadStats]’
/usr/include/c++/11/bits/stl_uninitialized.h:355:2: required from ‘_ForwardIterator std::__uninitialized_move_if_noexcept_a(_InputIterator, _InputIterator, _ForwardIterator, _Allocator&) [with _InputIterator = MultiThreadSubscriber::ThreadStats*; _ForwardIterator = MultiThreadSubscriber::ThreadStats*; _Allocator = std::allocator<MultiThreadSubscriber::ThreadStats>]’
/usr/include/c++/11/bits/vector.tcc:659:48: required from ‘void std::vector<_Tp, _Alloc>::_M_default_append(std::vector<_Tp, _Alloc>::size_type) [with _Tp = MultiThreadSubscriber::ThreadStats; _Alloc = std::allocator<MultiThreadSubscriber::ThreadStats>; std::vector<_Tp, _Alloc>::size_type = long unsigned int]’
/usr/include/c++/11/bits/stl_vector.h:940:4: required from ‘void std::vector<_Tp, _Alloc>::resize(std::vector<_Tp, _Alloc>::size_type) [with _Tp = MultiThreadSubscriber::ThreadStats; _Alloc = std::allocator<MultiThreadSubscriber::ThreadStats>; std::vector<_Tp, _Alloc>::size_type = long unsigned int]’
/home/yidds/YiDDS/multithreading_YIDDS/src/realtime_demo/src/multithread_subscriber.cpp:67:29: required from here
/usr/include/c++/11/bits/stl_uninitialized.h:138:72: error: static assertion failed: result type must be constructible from value type of input range
138 | static_assert(is_constructible<_ValueType2, decltype(*__first)>::value,
| ^~~~~
/usr/include/c++/11/bits/stl_uninitialized.h:138:72: note: ‘std::integral_constant<bool, false>::value’ evaluates to false
修改完给我完整的代码,要求功能基本不变,
最新发布