vector::push_back() demo

此程序接收用户输入的整数并将其添加到向量中,然后遍历向量并打印每个整数。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

#include <iostream>
#include <vector>
#include <string>

using namespace std;

int main()
{
    vector<int> ivec;
    int val;
    cout << "Enter some integers: ";
    while (cin >> val) {
        ivec.push_back(val);
    }

    for (auto val : ivec)
        cout << val << " ";
    cout << endl;

    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 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 修改完给我完整的代码,要求功能基本不变,
07-31
#include "wall_detection.h" // 全局变量定义 tf2_ros::Buffer* tfBuffer = nullptr; ros::Publisher pointcloud_pub; ros::Publisher lines_pub; ros::Publisher normals_pub; ros::Publisher nearest_point_pub; ros::Publisher tf_points_pub; ros::Publisher min_distance_pub; ros::Publisher nearest_distance_pub; //最近距离 // ros::Publisher left_front_nearest_distance_pub; // 左前侧垂直距离 ros::Publisher right_front_nearest_distance_pub; // 右前侧垂直距离 ros::Publisher left_back_nearest_distance_pub; // 左后侧垂直距离 ros::Publisher right_back_nearest_distance_pub; // 右后侧垂直距离 ros::Publisher foot_points_pub;//垂足 // bool isLeftWall(const DetectedLine& wall) { // 计算墙面中点Y坐标,大于0视为左侧墙面 return (wall.start.y + wall.end.y) / 2.0 > 0; } geometry_msgs::Point left_front_ref, left_back_ref; geometry_msgs::Point right_front_ref, right_back_ref; std::string choose_wall ; bool getPointParameters(ros::NodeHandle& nh) { // 为每个点的每个分量设置默认值 double left_front_default_x = 0.0, left_front_default_y = 0.0, left_front_default_z = 0.0; double left_back_default_x = 0.0, left_back_default_y = 0.0, left_back_default_z = 0.0; double right_front_default_x = 0.0, right_front_default_y = 0.0, right_front_default_z = 0.0; double right_back_default_x = 0.0, right_back_default_y = 0.0, right_back_default_z = 0.0; std::string inital_choose = "all"; // 获取左前点参数 nh.param<double>("left_front_ref_x", left_front_ref.x, left_front_default_x); nh.param<double>("left_front_ref_y", left_front_ref.y, left_front_default_y); nh.param<double>("left_front_ref_z", left_front_ref.z, left_front_default_z); // 获取左后点参数 nh.param<double>("left_back_ref_x", left_back_ref.x, left_back_default_x); nh.param<double>("left_back_ref_y", left_back_ref.y, left_back_default_y); nh.param<double>("left_back_ref_z", left_back_ref.z, left_back_default_z); // 获取右前点参数 nh.param<double>("right_front_ref_x", right_front_ref.x, right_front_default_x); nh.param<double>("right_front_ref_y", right_front_ref.y, right_front_default_y); nh.param<double>("right_front_ref_z", right_front_ref.z, right_front_default_z); // 获取右后点参数 nh.param<double>("right_back_ref_x", right_back_ref.x, right_back_default_x); nh.param<double>("right_back_ref_y", right_back_ref.y, right_back_default_y); nh.param<double>("right_back_ref_z", right_back_ref.z, right_back_default_z); nh.param<std::string>("choose_wall",choose_wall, "inital_choose"); // 打印获取的值 ROS_INFO("Left Front Ref: (%.2f, %.2f, %.2f)", left_front_ref.x, left_front_ref.y, left_front_ref.z); ROS_INFO("Left Back Ref: (%.2f, %.2f, %.2f)", left_back_ref.x, left_back_ref.y, left_back_ref.z); ROS_INFO("Right Front Ref: (%.2f, %.2f, %.2f)", right_front_ref.x, right_front_ref.y, right_front_ref.z); ROS_INFO("Right Back Ref: (%.2f, %.2f, %.2f)", right_back_ref.x, right_back_ref.y, right_back_ref.z); ROS_INFO("Choose_wall_type: %s", choose_wall.c_str()); return true; } void setupStaticTFs() { static tf2_ros::StaticTransformBroadcaster static_broadcaster; // 位于激光雷达左前方的tf坐标 geometry_msgs::TransformStamped left_tf; left_tf.header.stamp = ros::Time::now(); //left_tf.header.stamp = ros::Time(0); left_tf.header.frame_id = "base_footprint"; left_tf.child_frame_id = "laser_left_front"; left_tf.transform.translation.x = left_front_ref.x; left_tf.transform.translation.y = left_front_ref.y; left_tf.transform.translation.z = left_front_ref.z; left_tf.transform.rotation.x = 0.0; left_tf.transform.rotation.y = 0.0; left_tf.transform.rotation.z = 0.0; left_tf.transform.rotation.w = 1.0; // 位于激光雷达右前方的tf坐标 geometry_msgs::TransformStamped right_tf; right_tf.header.stamp = ros::Time::now(); //right_tf.header.stamp = ros::Time(0); right_tf.header.frame_id = "base_footprint"; right_tf.child_frame_id = "laser_right_front"; right_tf.transform.translation.x = right_front_ref.x; right_tf.transform.translation.y = right_front_ref.y; right_tf.transform.translation.z = right_front_ref.z; right_tf.transform.rotation.x = 0.0; right_tf.transform.rotation.y = 0.0; right_tf.transform.rotation.z = 0.0; right_tf.transform.rotation.w = 1.0; //左后方TF坐标 geometry_msgs::TransformStamped left_back_tf; left_back_tf.header.stamp = ros::Time::now(); //left_back_tf.header.stamp = ros::Time(0); left_back_tf.header.frame_id = "base_footprint"; left_back_tf.child_frame_id = "laser_left_back"; left_back_tf.transform.translation.x = left_back_ref.x; left_back_tf.transform.translation.y = left_back_ref.y; left_back_tf.transform.translation.z = left_back_ref.z; left_back_tf.transform.rotation.x = 0.0; left_back_tf.transform.rotation.y = 0.0; left_back_tf.transform.rotation.z = 0.0; left_back_tf.transform.rotation.w = 1.0; // 右后方TF坐标 geometry_msgs::TransformStamped right_back_tf; right_back_tf.header.stamp = ros::Time::now(); //right_back_tf.header.stamp = ros::Time(0); right_back_tf.header.frame_id = "base_footprint"; right_back_tf.child_frame_id = "laser_right_back"; right_back_tf.transform.translation.x = right_back_ref.x; right_back_tf.transform.translation.y = right_back_ref.y; right_back_tf.transform.translation.z = right_back_ref.z; right_back_tf.transform.rotation.x = 0.0; right_back_tf.transform.rotation.y = 0.0; right_back_tf.transform.rotation.z = 0.0; right_back_tf.transform.rotation.w = 1.0; static_broadcaster.sendTransform(left_tf); static_broadcaster.sendTransform(right_tf); static_broadcaster.sendTransform(left_back_tf); static_broadcaster.sendTransform(right_back_tf); } double pointToLineDistance(const geometry_msgs::Point& point, const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end) { Eigen::Vector3f pt(point.x, point.y, point.z); Eigen::Vector3f line_vec(line_end.x - line_start.x, line_end.y - line_start.y, line_end.z - line_start.z); Eigen::Vector3f pt_vec(pt.x() - line_start.x, pt.y() - line_start.y, pt.z() - line_start.z); double line_length = line_vec.norm(); if (line_length < 1e-6) { return pt_vec.norm(); } Eigen::Vector3f normalized_line = line_vec / line_length; double projection = pt_vec.dot(normalized_line); // 限制投影在直线范围内 projection = std::max(0.0, std::min(line_length, projection)); Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) + projection * normalized_line; return (pt - closest_point).norm(); } // 计算点到直线的垂足点 geometry_msgs::Point pointToLineProjection(const geometry_msgs::Point& point, const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end) { Eigen::Vector3f pt(point.x, point.y, point.z); Eigen::Vector3f line_vec(line_end.x - line_start.x, line_end.y - line_start.y, line_end.z - line_start.z); Eigen::Vector3f pt_vec(pt.x() - line_start.x, pt.y() - line_start.y, pt.z() - line_start.z); double line_length = line_vec.norm(); if (line_length < 1e-6) { return line_start; // 如果直线长度为零,返回起点 } Eigen::Vector3f normalized_line = line_vec / line_length; double projection = pt_vec.dot(normalized_line); // 限制投影在直线范围内 projection = std::max(0.0, std::min(line_length, projection)); Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) + projection * normalized_line; geometry_msgs::Point foot_point; foot_point.x = closest_point.x(); foot_point.y = closest_point.y(); foot_point.z = closest_point.z(); return foot_point; } void publishDetectedLines(const std::vector<DetectedLine>& lines, const std::string& frame_id) { visualization_msgs::Marker line_marker; line_marker.header.frame_id = frame_id; line_marker.header.stamp = ros::Time::now(); line_marker.ns = "detected_line"; line_marker.id = 0; line_marker.type = visualization_msgs::Marker::LINE_LIST; line_marker.pose.orientation.w = 1.0; line_marker.scale.x = 0.15; line_marker.color.r = 0.0; line_marker.color.g = 0.0; line_marker.color.b = 1.0; line_marker.color.a = 1.0; for (const auto& line : lines) { geometry_msgs::Point p1, p2; p1.x = line.start.x; p1.y = line.start.y; p1.z = line.start.z; p2.x = line.end.x; p2.y = line.end.y; p2.z = line.end.z; line_marker.points.push_back(p1); line_marker.points.push_back(p2); } lines_pub.publish(line_marker); visualization_msgs::MarkerArray normal_markers; int id = 0; for (const auto& line : lines) { visualization_msgs::Marker normal_marker; normal_marker.header.frame_id = frame_id; normal_marker.header.stamp = ros::Time::now(); normal_marker.ns = "normals"; normal_marker.id = id++; normal_marker.type = visualization_msgs::Marker::ARROW; normal_marker.action = visualization_msgs::Marker::ADD; normal_marker.pose.orientation.w = 1.0; normal_marker.scale.x = 0.02; normal_marker.scale.y = 0.04; normal_marker.scale.z = 0.0; normal_marker.color.r = 1.0; normal_marker.color.g = 0.0; normal_marker.color.b = 0.0; normal_marker.color.a = 1.0; geometry_msgs::Point mid_point; mid_point.x = (line.start.x + line.end.x) / 2.0; mid_point.y = (line.start.y + line.end.y) / 2.0; mid_point.z = (line.start.z + line.end.z) / 2.0; geometry_msgs::Point normal_end; normal_end.x = mid_point.x + line.normal.normal_x * 0.5; normal_end.y = mid_point.y + line.normal.normal_y * 0.5; normal_end.z = mid_point.z + line.normal.normal_z * 0.5; normal_marker.points.push_back(mid_point); normal_marker.points.push_back(normal_end); normal_markers.markers.push_back(normal_marker); } normals_pub.publish(normal_markers); } void publishNearestPointMarker(const geometry_msgs::Point& point,float distance,const std::string& frame_id, const std::string& ref_name) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "nearest_point" + ref_name; // marker.id = 0; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.pose.position = point; marker.pose.orientation.w = 1.0; marker.scale.x = 0.15; marker.scale.y = 0.15; marker.scale.z = 0.15; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0; marker.lifetime = ros::Duration(0.1); nearest_point_pub.publish(marker); //std_msgs::Float64 dist; //dist.data =(point.y < 0 ? -1.0 : 1.0) *(std::fabs(point.y) - 0.12); //dist.data = std::fabs(point.y); //nearest_distance_pub.publish(dist); //最近距离 std_msgs::Float64 dist_msg; if (ref_name == "left_front") { dist_msg.data = distance; left_front_nearest_distance_pub.publish(dist_msg); } else if (ref_name == "left_back") { dist_msg.data = distance; left_back_nearest_distance_pub.publish(dist_msg); } else if (ref_name == "right_front") { dist_msg.data = distance; right_front_nearest_distance_pub.publish(dist_msg); } else { dist_msg.data = distance; right_back_nearest_distance_pub.publish(dist_msg); } // } /***************************************************************/ void publishFootPointMarker(const geometry_msgs::Point& left_front_foot, //垂足标记 const geometry_msgs::Point& left_back_foot, //垂足标记 const geometry_msgs::Point& right_front_foot, const geometry_msgs::Point& right_back_foot, const std::string& frame_id) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "foot_points"; marker.id = 0; marker.type = visualization_msgs::Marker::POINTS; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.1; // 点的大小 marker.scale.y = 0.1; marker.color.a = 1.0; // 不透明度 // 添加左侧垂足点 - 蓝色 marker.points.push_back(left_front_foot); std_msgs::ColorRGBA color; color.r = 0.0; color.g = 0.0; color.b = 1.0; color.a = 1.0; marker.colors.push_back(color); // 添加右侧垂足点 - 绿色 marker.points.push_back(right_front_foot); color.r = 0.0; color.g = 1.0; color.b = 0.0; color.a = 1.0; marker.colors.push_back(color); // 添加左侧垂足点 - 蓝色 marker.points.push_back(left_back_foot); color.r = 0.0; color.g = 1.0; color.b = 1.0; color.a = 1.0; marker.colors.push_back(color); // 添加右侧垂足点 - 绿色 marker.points.push_back(right_back_foot); color.r = 1.0; color.g = 1.0; color.b = 0.0; color.a = 1.0; marker.colors.push_back(color); foot_points_pub.publish(marker); } /*******************************************************************/ void publishTFPoints(const geometry_msgs::Point& left_front_point, const geometry_msgs::Point& left_back_point, const geometry_msgs::Point& right_front_point, const geometry_msgs::Point& right_back_point, const std::string& frame_id) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "tf_point"; marker.id = 0; marker.type = visualization_msgs::Marker::POINTS; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.15; marker.scale.y = 0.15; marker.color.a = 1.0; // 左侧点 - 蓝色 geometry_msgs::Point p; p = left_front_point; marker.points.push_back(p); std_msgs::ColorRGBA c; c.r = 0.0; c.g = 0.0; c.b = 1.0; c.a = 1.0; marker.colors.push_back(c); // 右侧点 - 绿色 p = right_front_point; marker.points.push_back(p); c.r = 0.0; c.g = 1.0; c.b = 0.0; c.a = 1.0; marker.colors.push_back(c); // 左侧点 - 蓝色 p = left_back_point; marker.points.push_back(p); c.r = 0.0; c.g = 1.0; c.b = 1.0; c.a = 1.0; marker.colors.push_back(c); // 右侧点 - 绿色 p = right_back_point; marker.points.push_back(p); c.r = 1.0; c.g = 1.0; c.b = 0.0; c.a = 1.0; marker.colors.push_back(c); tf_points_pub.publish(marker); } void publishDistanceInfo(const std::string& frame_id, double left_front_dist, double left_back_dist, double right_front_dist, double right_back_dist, const geometry_msgs::Point& wall_point) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "distance_info"; marker.id = 0; marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; marker.action = visualization_msgs::Marker::ADD; marker.pose.position = wall_point; marker.pose.position.z += 0.5; // 在墙上点上方显示 marker.pose.orientation.w = 1.0; marker.scale.z = 0.2; // 文本大小 marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.color.a = 1.0; std::stringstream ss; ss << std::fixed << std::setprecision(2); ss << "Left_front_tf: " << left_front_dist << "m\n"; ss << "Left_back_tf: " << left_back_dist << "m\n"; ss << "Right_front_tf: " << right_front_dist << "m\n"; ss << "Right_back_tf: " << right_back_dist << "m\n"; if (left_front_dist < right_front_dist && left_back_dist < right_back_dist ) { ss << "Left is closer"; } else if (right_front_dist < left_front_dist && right_back_dist < left_back_dist ) { ss << "Right is closer"; } else { ss << "Equal distance"; } marker.text = ss.str(); min_distance_pub.publish(marker); } float min_four(float a ,float b , float c , float d ) { return std::min({a, b, c, d}); } void LidarCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { std::unordered_map<int, PointData> point_data_map; PointCloudT::Ptr cloud(new PointCloudT); cloud->header.frame_id = msg->header.frame_id; cloud->height = 1; cloud->is_dense = false; float min_distance = std::numeric_limits<float>::max(); int min_index = -1; int point_count = 0; const float min_angle1 = 30 * M_PI/180.0; const float max_angle1 = 150* M_PI/180.0; const float min_angle2 = -150 * M_PI/180.0; const float max_angle2 = -30 * M_PI/180.0; // 为参考点初始化最小距离 float left_front_min_distance = std::numeric_limits<float>::max(); int left_front_min_index = -1; geometry_msgs::Point left_front_nearest_point; float left_back_min_distance = std::numeric_limits<float>::max(); int left_back_min_index = -1; geometry_msgs::Point left_back_nearest_point; // 为参考点初始化最小距离 float right_front_min_distance = std::numeric_limits<float>::max(); int right_front_min_index = -1; geometry_msgs::Point right_front_nearest_point; float right_back_min_distance = std::numeric_limits<float>::max(); int right_back_min_index = -1; geometry_msgs::Point right_back_nearest_point; // 寻找最近点 for (size_t i = 0; i < msg->ranges.size(); ++i) { const float range = msg->ranges[i]; if (std::isnan(range)) continue; if (range < msg->range_min || range > msg->range_max) continue; const float angle = msg -> angle_min + i * msg -> angle_increment; //创建屏蔽条件检测 bool in_blocked_zone = true; float normalized_angle = angle; const float x = range * cos(angle); const float y = range * sin(angle); if (choose_wall == "left") { // 左侧 if (angle >= min_angle1 && angle <= max_angle1) { // if ( x > left_front_ref.x || x < left_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } } else if (choose_wall == "right") { // 右侧 if (angle >= min_angle2 && angle <= max_angle2) { // if ( x > right_front_ref.x || x < right_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } } else { if (angle >= min_angle1 && angle <= max_angle1) { // if ( x > left_front_ref.x || x < left_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } if (angle >= min_angle2 && angle <= max_angle2) { // if ( x > right_front_ref.x || x < right_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } } if (in_blocked_zone) continue; if (range < min_distance) { min_distance = range; min_index = i; } PointT point; point.x = range * cos(angle); point.y = range * sin(angle); point.z = 0.0; point.r = 0; point.g = 255; point.b = 0; PointData data; data.original_index = i; data.is_line_point = false; data.is_nearest = (i == min_index); point_data_map[point_count] = data; cloud->points.push_back(point); point_count++; } cloud->width = point_count; // 如果点云为空,直接返回 if (cloud->empty()) { ROS_WARN_THROTTLE(1.0, "No valid points found"); return; } std::vector<DetectedLine> detected_lines; pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); tree->setInputCloud(cloud); // 2. 执行欧几里得聚类 - 确保连续性 std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<PointT> ec; ec.setClusterTolerance(0.15); // 点间最大距离阈值(米) ec.setMinClusterSize(30); // 最小聚类点数 ec.setMaxClusterSize(10000); // 最大聚类点数 ec.setSearchMethod(tree); ec.setInputCloud(cloud); ec.extract(cluster_indices); ROS_INFO_THROTTLE(1.0, "Detected %zu point cloud clusters", cluster_indices.size()); std::vector<std::vector<uint8_t>> colors = { {255, 0, 0}, // 红 {0, 255, 0}, // 绿 {0, 0, 255}, // 蓝 {255, 255, 0}, // 黄 {0, 255, 255}, // 青 {255, 0, 255} // 紫 }; // 初始化点云颜色 for (auto& point : cloud->points) { point.r = 0; point.g = 255; point.b = 0; } // 3. 对每个聚类进行直线检测 for (size_t i = 0; i < cluster_indices.size(); i++) { const auto& cluster = cluster_indices[i]; // 创建当前聚类的点云 PointCloudT::Ptr cluster_cloud(new PointCloudT); for (const auto& idx : cluster.indices) { cluster_cloud->push_back((*cloud)[idx]); } // 为当前聚类的点着色 const auto& color = colors[i % colors.size()]; for (const auto& idx : cluster.indices) { cloud->points[idx].r = color[0]; cloud->points[idx].g = color[1]; cloud->points[idx].b = color[2]; } // 跳过点数过少的聚类 if (cluster_cloud->size() < 10) continue; pcl::SACSegmentation<PointT> seg; pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_LINE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(1000); seg.setDistanceThreshold(0.05); // 点到直线的最大距离阈值 seg.setInputCloud(cluster_cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size() < 10) continue; DetectedLine line; float min_proj = std::numeric_limits<float>::max(); float max_proj = std::numeric_limits<float>::lowest(); Eigen::Vector3f direction(coefficients->values[3], coefficients->values[4], 0.0f); direction.normalize(); // 计算直线的起点和终点 for (const auto& idx : inliers->indices) { Eigen::Vector3f pt( cluster_cloud->points[idx].x, cluster_cloud->points[idx].y, 0.0f ); float proj = pt.dot(direction); if (proj < min_proj) { min_proj = proj; line.start.x = pt.x(); line.start.y = pt.y(); line.start.z = 0.0f; } if (proj > max_proj) { max_proj = proj; line.end.x = pt.x(); line.end.y = pt.y(); line.end.z = 0.0f; } } // 计算法线方向 line.normal.normal_x = -direction.y(); line.normal.normal_y = direction.x(); line.normal.normal_z = 0.0f; line.direction = direction; detected_lines.push_back(line); // 标记直线点 for (const auto& inlier_idx : inliers->indices) { if (inlier_idx >= 0 && inlier_idx < cluster.indices.size()) { int original_idx = cluster.indices[inlier_idx]; if (original_idx >= 0 && original_idx < cloud->size()) { point_data_map[original_idx].is_line_point = true; } } } } // 更新点云颜色 for (int i = 0; i < cloud->size(); i++) { if (point_data_map[i].is_line_point) { cloud->points[i].r = 255; cloud->points[i].g = 255; cloud->points[i].b = 0; } } sensor_msgs::PointCloud2 cloud_msg; pcl::toROSMsg(*cloud, cloud_msg); cloud_msg.header = msg->header; std::vector<DetectedLine> transformed_lines; geometry_msgs::Point left_front_tf_point_map ,left_back_tf_point_map ; geometry_msgs::Point right_front_tf_point_map , right_back_tf_point_map ; bool left_front_tf_valid = false , left_back_tf_valid = false; bool right_front_tf_valid = false , right_back_tf_valid = false; if (tfBuffer) { try { if (!tfBuffer->canTransform("base_footprint", cloud_msg.header.frame_id, cloud_msg.header.stamp, ros::Duration(0.1))) { ROS_WARN_THROTTLE(1.0, "TF transform not available"); return; } // 转换点云 sensor_msgs::PointCloud2 transformed_cloud; tfBuffer->transform(cloud_msg, transformed_cloud, "base_footprint"); transformed_cloud.header.stamp = ros::Time::now(); transformed_cloud.header.frame_id = "base_footprint"; pointcloud_pub.publish(transformed_cloud); // 转换检测到的直线 if (!detected_lines.empty()) { for (auto& line : detected_lines) { geometry_msgs::PointStamped laser_start, map_start; laser_start.header = msg->header; laser_start.point.x = line.start.x; laser_start.point.y = line.start.y; laser_start.point.z = 0.0f; tfBuffer->transform(laser_start, map_start, "base_footprint"); geometry_msgs::PointStamped laser_end, map_end; laser_end.header = msg->header; laser_end.point.x = line.end.x; laser_end.point.y = line.end.y; laser_end.point.z = 0.0f; tfBuffer->transform(laser_end, map_end, "base_footprint"); geometry_msgs::Vector3Stamped laser_normal, map_normal; laser_normal.header = msg->header; laser_normal.vector.x = line.normal.normal_x; laser_normal.vector.y = line.normal.normal_y; laser_normal.vector.z = 0.0f; tfBuffer->transform(laser_normal, map_normal, "base_footprint"); DetectedLine transformed_line; transformed_line.start.x = map_start.point.x; transformed_line.start.y = map_start.point.y; transformed_line.start.z = map_start.point.z; transformed_line.end.x = map_end.point.x; transformed_line.end.y = map_end.point.y; transformed_line.end.z = map_end.point.z; transformed_line.normal.normal_x = map_normal.vector.x; transformed_line.normal.normal_y = map_normal.vector.y; transformed_line.normal.normal_z = map_normal.vector.z; transformed_lines.push_back(transformed_line); } publishDetectedLines(transformed_lines, "base_footprint"); } // 获取TF参考点在base_footprint中的位置 try { geometry_msgs::PointStamped left_front_tf_laser, left_front_tf_map; left_front_tf_laser.header.frame_id = "laser_left_front"; left_front_tf_laser.header.stamp = ros::Time(0); left_front_tf_laser.point.x = left_front_ref.x; left_front_tf_laser.point.y = left_front_ref.y; left_front_tf_laser.point.z = left_front_ref.z; tfBuffer->transform(left_front_tf_laser, left_front_tf_map, "base_footprint"); left_front_tf_point_map = left_front_tf_map.point; left_front_tf_valid = true; geometry_msgs::PointStamped left_back_tf_laser, left_back_tf_map; left_back_tf_laser.header.frame_id = "laser_left_back"; left_back_tf_laser.header.stamp = ros::Time(0); left_back_tf_laser.point.x = left_back_ref.x; left_back_tf_laser.point.y = left_back_ref.y; left_back_tf_laser.point.z = left_back_ref.z; tfBuffer->transform(left_back_tf_laser, left_back_tf_map, "base_footprint"); left_back_tf_point_map = left_back_tf_map.point; left_back_tf_valid = true; geometry_msgs::PointStamped right_front_tf_laser, right_front_tf_map; right_front_tf_laser.header.frame_id = "laser_right_front"; right_front_tf_laser.header.stamp = ros::Time(0); right_front_tf_laser.point.x = right_front_ref.x; right_front_tf_laser.point.y = right_front_ref.y; right_front_tf_laser.point.z = right_front_ref.z; tfBuffer->transform(right_front_tf_laser, right_front_tf_map, "base_footprint"); right_front_tf_point_map = right_front_tf_map.point; right_front_tf_valid = true; geometry_msgs::PointStamped right_back_tf_laser, right_back_tf_map; right_back_tf_laser.header.frame_id = "laser_right_back"; right_back_tf_laser.header.stamp = ros::Time(0); right_back_tf_laser.point.x = right_back_ref.x; right_back_tf_laser.point.y = right_back_ref.y; right_back_tf_laser.point.z = right_back_ref.z; tfBuffer->transform(right_back_tf_laser, right_back_tf_map, "base_footprint"); right_back_tf_point_map = right_back_tf_map.point; right_back_tf_valid = true; publishTFPoints(left_front_tf_point_map, left_back_tf_point_map, right_front_tf_point_map, right_back_tf_point_map, "base_footprint"); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what()); } } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what()); } } if ( ! transformed_lines.empty() && left_front_tf_valid && left_back_tf_valid && right_front_tf_valid && right_back_tf_valid) { std::vector<DetectedLine>left_walls; std::vector<DetectedLine>right_walls; for (const auto& wall : transformed_lines) { if (isLeftWall(wall)) { left_walls.push_back(wall); } else { right_walls.push_back(wall); } } double left_front_min_dist = std::numeric_limits<double>::max(); double left_back_min_dist = std::numeric_limits<double>::max(); geometry_msgs::Point left_front_foot , left_back_foot; geometry_msgs::Point left_front_wall_point, left_back_wall_point; for (const auto& wall : left_walls) { geometry_msgs::Point start_point, end_point; start_point.x = wall.start.x; start_point.y = wall.start.y; start_point.z = wall.start.z; end_point.x = wall.end.x; end_point.y = wall.end.y; end_point.z = wall.end.z; double dist_front = pointToLineDistance( left_front_tf_point_map, start_point, end_point); // 计算垂足点 geometry_msgs::Point foot_front = pointToLineProjection( left_front_tf_point_map, start_point, end_point ); if (dist_front < left_front_min_dist) { left_front_min_dist = dist_front; left_front_foot = foot_front; left_front_wall_point = foot_front; } double dist_back = pointToLineDistance( left_back_tf_point_map, start_point, end_point); // 计算垂足点 geometry_msgs::Point foot_back = pointToLineProjection( left_back_tf_point_map, start_point, end_point ); if (dist_back < left_back_min_dist) { left_back_min_dist = dist_back; left_back_foot = foot_back; left_back_wall_point = foot_back; } } //右墙 double right_front_min_dist = std::numeric_limits<double>::max(); double right_back_min_dist = std::numeric_limits<double>::max(); geometry_msgs::Point right_front_foot, right_back_foot; geometry_msgs::Point right_front_wall_point, right_back_wall_point; for (const auto& wall : right_walls) { geometry_msgs::Point start_point, end_point; start_point.x = wall.start.x; start_point.y = wall.start.y; start_point.z = wall.start.z; end_point.x = wall.end.x; end_point.y = wall.end.y; end_point.z = wall.end.z; // 计算右前参考点 double dist_front = pointToLineDistance( right_front_tf_point_map, start_point, end_point ); // 计算垂足点 geometry_msgs::Point foot_front = pointToLineProjection( right_front_tf_point_map, start_point, end_point ); if (dist_front < right_front_min_dist) { right_front_min_dist = dist_front; right_front_foot = foot_front; right_front_wall_point = foot_front; } // 计算右后参考点 double dist_back = pointToLineDistance( right_back_tf_point_map, start_point, end_point ); // 计算垂足点 geometry_msgs::Point foot_back = pointToLineProjection( right_back_tf_point_map, start_point, end_point ); if (dist_back < right_back_min_dist) { right_back_min_dist = dist_back; right_back_foot = foot_back; right_back_wall_point = foot_back; } } std_msgs::Float64 left_front_distance_msg, left_back_distance_msg , right_front_distance_msg , right_back_distance_msg; // 左前距离 double left_front_vertical_dist = (left_front_min_dist == std::numeric_limits<double>::max()) ? 10.0 : left_front_min_dist; left_front_distance_msg.data = left_front_vertical_dist; //left_front_nearest_distance_pub.publish(left_front_distance_msg); // 左后距离 double left_back_vertical_dist = (left_back_min_dist == std::numeric_limits<double>::max()) ? 10.0 : left_back_min_dist; left_back_distance_msg.data = left_back_vertical_dist; //left_back_nearest_distance_pub.publish(left_back_distance_msg); // 右前距离 double right_front_vertical_dist = (right_front_min_dist == std::numeric_limits<double>::max()) ? 10.0 : right_front_min_dist; right_front_distance_msg.data = right_front_vertical_dist; //right_front_nearest_distance_pub.publish(right_front_distance_msg); // 右后距离 double right_back_vertical_dist = (right_back_min_dist == std::numeric_limits<double>::max()) ? 10.0 : right_back_min_dist; right_back_distance_msg.data = right_back_vertical_dist; //right_back_nearest_distance_pub.publish(right_back_distance_msg); // 发布垂足点 publishFootPointMarker(left_front_foot, left_back_foot, right_front_foot, right_back_foot, "base_footprint"); // 发布距离信息 geometry_msgs::Point wall_mid_point; wall_mid_point.x = (left_front_wall_point.x + right_front_wall_point.x) / 2; wall_mid_point.y = (left_front_wall_point.y + right_front_wall_point.y) / 2; wall_mid_point.z = 0; float dist_min = min_four(left_front_vertical_dist,left_back_vertical_dist,right_front_vertical_dist,right_back_vertical_dist); if ( dist_min == left_front_vertical_dist ) { nearest_distance_pub.publish(left_front_distance_msg); } else if ( dist_min == left_back_vertical_dist ) { nearest_distance_pub.publish(left_back_distance_msg); } else if ( dist_min == right_front_vertical_dist ) { nearest_distance_pub.publish(right_front_distance_msg); } else { nearest_distance_pub.publish(right_back_distance_msg); } publishDistanceInfo("base_footprint", left_front_min_distance, left_back_min_distance, right_front_min_distance, right_back_min_distance, wall_mid_point); ROS_INFO_THROTTLE(1.0, "Left Front TF to wall: %.3f m, Left Back TF to wall: %.3f m , Right Front TF to wall: %.3f m, Right Bcak TF to wall: %.3f m", left_front_vertical_dist , left_back_vertical_dist ,right_front_vertical_dist ,right_back_vertical_dist ); } }在原本的代码中, 对“left_back_tf.transform.translation.x”等是直接赋值且“left_front_tf_laser.point.x = 0”等,是可以观察到tf坐标的,为什么通过launch赋值就观察不到了
07-18
报错:guo@guo-Dell-G15-5520:~/g2o仿真/src/build$ make Consolidate compiler generated dependencies of target g2o_demo [ 50%] Building CXX object CMakeFiles/g2o_demo.dir/optimize.cc.o /home/guo/g2o仿真/src/optimize.cc: In member function ‘void TrajectoryOptimizer::optimizeTrajectory(std::vector<Eigen::Transform<double, 3, 1> >&)’: /home/guo/g2o仿真/src/optimize.cc:28:62: error: call of overloaded ‘make_unique<g2o::BlockSolverX>(std::remove_reference<std::unique_ptr<g2o::LinearSolverDense<Eigen::Matrix<double, -1, -1> >, std::default_delete<g2o::LinearSolverDense<Eigen::Matrix<double, -1, -1> > > >&>::type)’ is ambiguous 28 | make_unique<g2o::BlockSolverX>(move(linearSolver))); | ^ In file included from /usr/include/c++/9/memory:80, from /opt/ros/noetic/include/g2o/core/robust_kernel.h:30, from /opt/ros/noetic/include/g2o/core/base_binary_edge.h:34, from /home/guo/g2o仿真/src/optimize.cc:6: /usr/include/c++/9/bits/unique_ptr.h:856:5: note: candidate: ‘typename std::_MakeUniq<_Tp>::__single_object std::make_unique(_Args&& ...) [with _Tp = g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> >; _Args = {std::unique_ptr<g2o::LinearSolverDense<Eigen::Matrix<double, -1, -1, 0, -1, -1> >, std::default_delete<g2o::LinearSolverDense<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > >}; typename std::_MakeUniq<_Tp>::__single_object = std::unique_ptr<g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> >, std::default_delete<g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> > > >]’ 856 | make_unique(_Args&&... __args) | ^~~~~~~~~~~ In file included from /opt/ros/noetic/include/g2o/core/base_binary_edge.h:36, from /home/guo/g2o仿真/src/optimize.cc:6: /opt/ros/noetic/include/g2o/stuff/misc.h:52:20: note: candidate: ‘std::unique_ptr<_Tp> g2o::make_unique(ArgTs&& ...) [with T = g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> >; ArgTs = {std::unique_ptr<g2o::LinearSolverDense<Eigen::Matrix<double, -1, -1, 0, -1, -1> >, std::default_delete<g2o::LinearSolverDense<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > >}]’ 52 | std::unique_ptr<T> make_unique(ArgTs&& ...args) | ^~~~~~~~~~~ make[2]: *** [CMakeFiles/g2o_demo.dir/build.make:76:CMakeFiles/g2o_demo.dir/optimize.cc.o] 错误 1 make[1]: *** [CMakeFiles/Makefile2:673:CMakeFiles/g2o_demo.dir/all] 错误 2 make: *** [Makefile:146:all] 错误 2
最新发布
08-03
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值