get_set_priority.cpp

 
#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
/*! \file main.c \brief Implementation of ble datatrans server demo. \version 2023-07-20, V1.0.0, firmware for GD32VW55x */ /* Copyright (c) 2023, GigaDevice Semiconductor Inc. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include <stdint.h> #include "dbg_print.h" #include "gd32vw55x_platform.h" #include "wrapper_os.h" #include "ble_adapter.h" #include "ble_adv.h" #include "ble_conn.h" #include "ble_utils.h" #include "ble_export.h" #include "ble_sec.h" #include "ble_datatrans_srv.h" #include "app_uart.h" /* Device name */ #define DEV_NAME "GD-BLE-DEV" /* Device name length*/ #define DEV_NAME_LEN strlen(dev_name) /* Advertising parameters */ typedef struct { uint8_t adv_idx; /*!< Advertising id. use to stop advertising */ ble_adv_state_t adv_state; /*!< Advertising state */ } app_adv_param_t; /* Definitions of the different task priorities */ enum { BLE_STACK_TASK_PRIORITY = OS_TASK_PRIORITY(2), /*!< Priority of the BLE stack task */ BLE_APP_TASK_PRIORITY = OS_TASK_PRIORITY(1), /*!< Priority of the BLE APP task */ }; /* Definitions of the different BLE task stack size requirements */ enum { BLE_STACK_TASK_STACK_SIZE = 768, /*!< BLE stack task stack size */ BLE_APP_TASK_STACK_SIZE = 512, /*!< BLE APP task stack size */ }; /* Device name array*/ char dev_name[] = {DEV_NAME}; /* advertising env*/ app_adv_param_t app_adv_env = {0}; /* connection index */ uint8_t conn_idx = 0; /*! \brief Start advertising \param[in] p_adv: pointer to BLE advertising set \param[out] none \retval none */ static ble_status_t app_adv_start(void) { ble_data_t adv_data = {0}; ble_data_t adv_scanrsp_data = {0}; ble_adv_data_set_t adv = {0}; ble_adv_data_set_t scan_rsp = {0}; uint8_t data[BLE_GAP_LEGACY_ADV_MAX_LEN] = {0}; uint8_t idx = 0; data[idx++] = 2; data[idx++] = BLE_AD_TYPE_FLAGS; data[idx++] = BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED | BLE_GAP_ADV_FLAG_LE_GENERAL_DISC_MODE; data[idx++] = DEV_NAME_LEN + 1; data[idx++] = 0x09; memcpy(&data[idx], dev_name, DEV_NAME_LEN); idx += DEV_NAME_LEN; adv_data.len = idx; adv_data.p_data = data; adv_scanrsp_data.len = idx - 3; adv_scanrsp_data.p_data = &data[3]; adv.data_force = true; adv.data.p_data_force = &adv_data; scan_rsp.data_force = true; scan_rsp.data.p_data_force = &adv_scanrsp_data; return ble_adv_start(app_adv_env.adv_idx, &adv, &scan_rsp, NULL); } /*! \brief Callback function to handle BLE advertising events \param[in] adv_evt: BLE advertising event type \param[in] p_data: pointer to BLE advertising event data \param[in] p_context: context data used when create advertising \param[out] none \retval none */ static void app_adv_mgr_evt_hdlr(ble_adv_evt_t adv_evt, void *p_data, void *p_context) { if (adv_evt == BLE_ADV_EVT_STATE_CHG) { ble_adv_state_chg_t *p_chg = (ble_adv_state_chg_t *)p_data; ble_adv_state_t old_state = app_adv_env.adv_state; app_print("%s state change 0x%x ==> 0x%x, reason 0x%x\r\n", __func__, old_state, p_chg->state, p_chg->reason); app_adv_env.adv_state = p_chg->state; if ((p_chg->state == BLE_ADV_STATE_CREATE) && (old_state == BLE_ADV_STATE_CREATING)) { app_adv_env.adv_idx = p_chg->adv_idx; app_adv_start(); } } } /*! \brief Create an advertising \param[in] p_param: pointer to advertising parameters \param[out] none \retval ble_status_t: BLE_ERR_NO_ERROR on success, otherwise an error code */ static ble_status_t app_adv_create(void) { ble_adv_param_t adv_param = {0}; adv_param.param.disc_mode = BLE_GAP_ADV_MODE_GEN_DISC; adv_param.param.own_addr_type = BLE_GAP_LOCAL_ADDR_STATIC; adv_param.param.type = BLE_GAP_ADV_TYPE_LEGACY; adv_param.param.prop = BLE_GAP_ADV_PROP_UNDIR_CONN; adv_param.param.filter_pol = BLE_GAP_ADV_ALLOW_SCAN_ANY_CON_ANY; adv_param.param.ch_map = 0x07; adv_param.param.primary_phy = BLE_GAP_PHY_1MBPS; adv_param.param.adv_intv_min = 160; adv_param.param.adv_intv_max = 160; adv_param.restart_after_disconn = true; return ble_adv_create(&adv_param, app_adv_mgr_evt_hdlr, NULL); } /*! \brief Function to execute app code after stack ready \param[in] none \param[out] none \retval none */ void ble_task_ready(void) { app_adv_create(); } /*! \brief Callback function to handle BLE adapter events \param[in] event: BLE adapter event type \param[in] p_data: pointer to BLE adapter event data \param[out] none \retval none */ static void app_adp_evt_handler(ble_adp_evt_t event, ble_adp_data_u *p_data) { uint8_t i = 0; if (event == BLE_ADP_EVT_ENABLE_CMPL_INFO) { if (p_data->adapter_info.status == BLE_ERR_NO_ERROR) { app_print("=== Adapter enable success ===\r\n"); app_print("hci_ver 0x%x, hci_subver 0x%x, lmp_ver 0x%x, lmp_subver 0x%x, manuf_name 0x%x\r\n", p_data->adapter_info.version.hci_ver, p_data->adapter_info.version.hci_subver, p_data->adapter_info.version.lmp_ver, p_data->adapter_info.version.lmp_subver, p_data->adapter_info.version.manuf_name); app_print("adv_set_num %u, min_tx_pwr %d, max_tx_pwr %d, max_adv_data_len %d \r\n", p_data->adapter_info.adv_set_num, p_data->adapter_info.tx_pwr_range.min_tx_pwr, p_data->adapter_info.tx_pwr_range.max_tx_pwr, p_data->adapter_info.max_adv_data_len); app_print("sugg_max_tx_octets %u, sugg_max_tx_time %u \r\n", p_data->adapter_info.sugg_dft_data.sugg_max_tx_octets, p_data->adapter_info.sugg_dft_data.sugg_max_tx_time); app_print("loc irk:"); for (i = 0; i < BLE_GAP_KEY_LEN; i++) { app_print(" %02x", p_data->adapter_info.loc_irk_info.irk[i]); } app_print("\r\n"); app_print("identity addr %02X:%02X:%02X:%02X:%02X:%02X \r\n ", p_data->adapter_info.loc_irk_info.identity.addr[5], p_data->adapter_info.loc_irk_info.identity.addr[4], p_data->adapter_info.loc_irk_info.identity.addr[3], p_data->adapter_info.loc_irk_info.identity.addr[2], p_data->adapter_info.loc_irk_info.identity.addr[1], p_data->adapter_info.loc_irk_info.identity.addr[0]); app_print("=== BLE Adapter enable complete ===\r\n"); ble_task_ready(); } else { app_print("=== BLE Adapter enable fail ===\r\n"); } } } /*! \brief Init adapter application module \param[in] none \param[out] none \retval none */ void app_adapter_init(void) { ble_adp_callback_register(app_adp_evt_handler); } /*! \brief Send security request \param[in] conidx: connection index \param[out] none \retval none */ void app_sec_send_security_req(uint8_t conidx) { uint8_t auth = BLE_GAP_AUTH_REQ_NO_MITM_NO_BOND; if (ble_sec_security_req(conidx, auth) != BLE_ERR_NO_ERROR) { app_print("app_sec_send_security_req fail! \r\n"); } } /*! \brief Callback function to handle BLE connection event \param[in] event: BLE connection event type \param[in] p_data: pointer to BLE connection event data \param[out] none \retval none */ static void app_conn_evt_handler(ble_conn_evt_t event, ble_conn_data_u *p_data) { switch (event) { case BLE_CONN_EVT_STATE_CHG: { if (p_data->conn_state.state == BLE_CONN_STATE_DISCONNECTD) { app_print("disconnected. conn idx: %u, conn_hdl: 0x%x reason 0x%x\r\n", p_data->conn_state.info.discon_info.conn_idx, p_data->conn_state.info.discon_info.conn_hdl, p_data->conn_state.info.discon_info.reason); } else if (p_data->conn_state.state == BLE_CONN_STATE_CONNECTED) { app_print("connect success. conn idx:%u, conn_hdl:0x%x \r\n", p_data->conn_state.info.conn_info.conn_idx, p_data->conn_state.info.conn_info.conn_hdl); if (p_data->conn_state.info.conn_info.role == BLE_SLAVE) { app_sec_send_security_req(p_data->conn_state.info.conn_info.conn_idx); } conn_idx = p_data->conn_state.info.conn_info.conn_idx; } } break; case BLE_CONN_EVT_PARAM_UPDATE_IND: { app_print("conn idx %u, intv_min 0x%x, intv_max 0x%x, latency %u, supv_tout %u\r\n", p_data->conn_param_req_ind.conn_idx, p_data->conn_param_req_ind.intv_min, p_data->conn_param_req_ind.intv_max, p_data->conn_param_req_ind.latency, p_data->conn_param_req_ind.supv_tout); ble_conn_param_update_cfm(p_data->conn_param_req_ind.conn_idx, true, 2, 4); } break; case BLE_CONN_EVT_PARAM_UPDATE_RSP: { app_print("conn idx %u, param update result status: 0x%x\r\n", p_data->conn_param_rsp.conn_idx, p_data->conn_param_rsp.status); } break; case BLE_CONN_EVT_PARAM_UPDATE_INFO: { app_print("conn idx %u, param update ind: interval %d, latency %d, sup to %d\r\n", p_data->conn_params.conn_idx, p_data->conn_params.interval, p_data->conn_params.latency, p_data->conn_params.supv_tout); } break; case BLE_CONN_EVT_PKT_SIZE_SET_RSP: { app_print("conn idx %u, packet size set status 0x%x\r\n", p_data->pkt_size_set_rsp.conn_idx, p_data->pkt_size_set_rsp.status); } break; case BLE_CONN_EVT_PKT_SIZE_INFO: { app_print("le pkt size info: conn idx %u, tx oct %d, tx time %d, rx oct %d, rx time %d\r\n", p_data->pkt_size_info.conn_idx, p_data->pkt_size_info.max_tx_octets, p_data->pkt_size_info.max_tx_time, p_data->pkt_size_info.max_rx_octets, p_data->pkt_size_info.max_rx_time); } break; case BLE_CONN_EVT_NAME_GET_IND: { ble_conn_name_get_cfm(p_data->name_get_ind.conn_idx, 0, p_data->name_get_ind.token, DEV_NAME_LEN, (uint8_t *)dev_name, DEV_NAME_LEN); } break; case BLE_CONN_EVT_APPEARANCE_GET_IND: { ble_conn_appearance_get_cfm(p_data->appearance_get_ind.conn_idx, 0, p_data->appearance_get_ind.token, 0); } break; default: break; } } /*! \brief Callback function to handle @ref BLE_SEC_EVT_PAIRING_REQ_IND event \param[in] p_ind: pointer to the pairing request indication data \param[out] none \retval none */ static void app_pairing_req_hdlr(ble_gap_pairing_req_ind_t *p_ind) { ble_gap_pairing_param_t param = {0}; param.auth = BLE_GAP_AUTH_MASK_NONE; param.iocap = BLE_GAP_IO_CAP_NO_IO; param.key_size = 16; param.ikey_dist = BLE_GAP_KDIST_IDKEY | BLE_GAP_KDIST_SIGNKEY | BLE_GAP_KDIST_ENCKEY; param.rkey_dist = BLE_GAP_KDIST_IDKEY | BLE_GAP_KDIST_SIGNKEY | BLE_GAP_KDIST_ENCKEY; ble_sec_pairing_req_cfm(p_ind->conn_idx, true, &param, BLE_GAP_NO_SEC); } /*! \brief Callback function to handle @ref BLE_SEC_EVT_PAIRING_FAIL_INFO event \param[in] p_info: pointer to the pairing fail information data \param[out] none \retval none */ static void app_pairing_fail_hdlr(ble_sec_pairing_fail_t *p_info) { app_print("pairing fail reason 0x%x\r\n", p_info->param.reason); } /*! \brief Callback function to handle @ref BLE_SEC_EVT_PAIRING_SUCCESS_INFO event \param[in] p_info: pointer to the pairing success information data \param[out] none \retval none */ static void app_pairing_success_hdlr(ble_sec_pairing_success_t *p_info) { app_print("conn_idx %u pairing success, level 0x%x ltk_present %d sc %d\r\n", p_info->conidx, p_info->bond_info.pairing_lvl, p_info->bond_info.enc_key_present, p_info->sc); } /*! \brief Callback function to handle BLE security events \param[in] event: BLE security event type \param[in] p_data: pointer to the BLE security event data \param[out] none \retval none */ static void app_sec_evt_handler(ble_sec_evt_t event, ble_sec_data_u *p_data) { switch (event) { case BLE_SEC_EVT_PAIRING_REQ_IND: app_pairing_req_hdlr((ble_gap_pairing_req_ind_t *)p_data); break; case BLE_SEC_EVT_PAIRING_SUCCESS_INFO: app_pairing_success_hdlr((ble_sec_pairing_success_t *)p_data); break; case BLE_SEC_EVT_PAIRING_FAIL_INFO: app_pairing_fail_hdlr((ble_sec_pairing_fail_t *)p_data); break; default: break; } } /*! \brief Callback function to handle data received by datatrans server service \param[in] data_len: received data length \param[in] p_data: pointer to received data \param[out] none \retval none */ void app_datatrans_srv_rx_callback(uint16_t data_len, uint8_t *p_data) { uint8_t *p_str = sys_malloc(data_len + 1); if (p_str) { app_print("datatrans srv receive data: \r\n"); memset(p_str, 0, data_len + 1); memcpy(p_str, p_data, data_len); app_print("%s\r\n", p_str); sys_mfree(p_str); } } /*! \brief Init application security module \param[in] none \param[out] none \retval none */ void app_sec_mgr_init(void) { ble_sec_callback_register(app_sec_evt_handler); } /*! \brief Init APP connection manager module \param[in] none \param[out] none \retval none */ void app_conn_mgr_init(void) { ble_conn_callback_register(app_conn_evt_handler); } /*! \brief Init BLE component modules needed \param[in] none \param[out] none \retval none */ void ble_init(void) { ble_init_param_t param = {0}; ble_os_api_t os_interface = { .os_malloc = sys_malloc, .os_calloc = sys_calloc, .os_mfree = sys_mfree, .os_memset = sys_memset, .os_memcpy = sys_memcpy, .os_memcmp = sys_memcmp, .os_task_create = sys_task_create, .os_task_init_notification = sys_task_init_notification, .os_task_wait_notification = sys_task_wait_notification, .os_task_notify = sys_task_notify, .os_task_delete = sys_task_delete, .os_ms_sleep = sys_ms_sleep, .os_current_task_handle_get = sys_current_task_handle_get, .os_queue_init = sys_queue_init, .os_queue_free = sys_queue_free, .os_queue_write = sys_queue_write, .os_queue_read = sys_queue_read, .os_random_bytes_get = sys_random_bytes_get, }; ble_power_on(); param.role = BLE_GAP_ROLE_PERIPHERAL; param.keys_user_mgr = false; param.pairing_mode = BLE_GAP_PAIRING_SECURE_CONNECTION | BLE_GAP_PAIRING_LEGACY; param.privacy_cfg = BLE_GAP_PRIV_CFG_PRIV_EN_BIT; param.ble_task_stack_size = BLE_STACK_TASK_STACK_SIZE; param.ble_task_priority = BLE_STACK_TASK_PRIORITY; param.ble_app_task_stack_size = BLE_APP_TASK_STACK_SIZE; param.ble_app_task_priority = BLE_APP_TASK_PRIORITY; param.name_perm = BLE_GAP_WRITE_NOT_ENC; param.appearance_perm = BLE_GAP_WRITE_NOT_ENC; param.en_cfg = 0; param.p_os_api = &os_interface; ble_sw_init(&param); app_adapter_init(); app_conn_mgr_init(); app_sec_mgr_init(); ble_datatrans_srv_init(); ble_datatrans_srv_rx_cb_reg(app_datatrans_srv_rx_callback); /* The BLE interrupt must be enabled after ble_sw_init. */ ble_irq_enable(); } /*! \brief Main entry point This function is called right after the booting process has completed. \param[in] none \param[out] none \retval none */ int main(void) { sys_os_init(); platform_init(); app_uart_init(); ble_init(); sys_os_start(); for ( ; ; ); } /*! \file app_uart.c \brief APP UART for GD32VW55x SDK. \version 2023-07-20, V1.0.0, firmware for GD32VW55x */ /* Copyright (c) 2023, GigaDevice Semiconductor Inc. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include <ctype.h> #include "gd32vw55x.h" #include <stdio.h> #include "app_uart.h" #include "wrapper_os.h" #include "dbg_print.h" #include "wakelock.h" #include "log_uart.h" #include "ble_datatrans_srv.h" /* Priority of the uart task */ #define UART_TASK_PRIORITY OS_TASK_PRIORITY(4) /* Uart queque size */ #define UART_QUEUE_SIZE 3 /* Message format */ struct uart_msg { uint16_t len; /*!< Length, in bytes, of the message */ void *data; /*!< Pointer to the message */ }; extern uint8_t conn_idx; /* Uart queque */ static os_queue_t uart_queue; /* Uart cyclic buffer */ static cyclic_buf_t uart_cyc_buf; /* Uart RX buffer */ char uart_rx_buf[UART_BUFFER_SIZE]; /* Uart RX buffer index*/ uint32_t uart_index = 0; /*! \brief app uart rx handle \param[in] uart_cyc_buf: uart cyclic buffer \param[out] buf: data buf \param[in] len: data length \retval none */ static void app_uart_rx_handle_done(cyclic_buf_t *uart_cyc_buf, uint8_t *buf, uint16_t *len) { if (*len > cyclic_buf_count(uart_cyc_buf)) { *len = cyclic_buf_count(uart_cyc_buf); } if (buf == NULL) { cyclic_buf_drop(uart_cyc_buf, *len); } else { cyclic_buf_read(uart_cyc_buf, buf, *len); } } /*! \brief uart message precess \param[in] msg: uart message \retval none */ static void app_uart_msg_process(struct uart_msg *msg) { cyclic_buf_t *p_cyclic_buf = (cyclic_buf_t *)msg->data; char *command; command = sys_malloc(msg->len + 1); if (command == NULL) { app_print("No buffer alloc for uart msg !\r\n"); return; } app_uart_rx_handle_done((cyclic_buf_t *)msg->data, (uint8_t *)command, &msg->len); command[msg->len] = '\0'; if (ble_datatrans_srv_tx(conn_idx, (uint8_t *)command, msg->len) == BLE_ERR_NO_ERROR) { app_print("datatrans srv send data: \r\n"); app_print("%s\r\n", command); } sys_mfree(command); return; } /*! \brief uart task \param[in] param: \retval none */ static void app_uart_task(void *param) { struct uart_msg msg; for (;;) { sys_queue_read(&uart_queue, &msg, -1, false); app_uart_msg_process(&msg); } } /*! \brief uart message send \param[in] msg_data: message data \param[in] len: message length \retval success 0 */ int app_uart_send(void *msg_data, uint16_t len) { struct uart_msg msg; msg.len = len; msg.data = msg_data; return sys_queue_write(&uart_queue, &msg, 0, true); } /*! \brief uart rx data handler \param[in] none \retval none */ static void rx_handler(void) { // uart_index - 2 to remove 0x0d and 0x0a when using Husky if (uart_index > 2) { if (app_uart_send((void *)(&uart_cyc_buf), uart_index - 2) == 0) { if (cyclic_buf_write(&uart_cyc_buf, (uint8_t *)uart_rx_buf, uart_index - 2) == false) { dbg_print(ERR, "uart cyclic buffer full\r\n"); } } else { /* queue was full */ dbg_print(ERR, "queue full\r\n"); /* TODO: report 'message ignored' status */ } } uart_index = 0; } /*! \brief app uart rx irq handler \param[in] uart_port: uart port \retval none */ static void app_uart_rx_irq_hdl(uint32_t uart_port) { usart_interrupt_disable(uart_port, USART_INT_RBNE); while (1) { // We should have chance to check overflow error // Otherwise it may cause dead loop handle rx interrupt if (RESET != usart_flag_get(uart_port, USART_FLAG_ORERR)) { usart_flag_clear(uart_port, USART_FLAG_ORERR); } if ((RESET != usart_flag_get(uart_port, USART_FLAG_RBNE))) { uart_rx_buf[uart_index++] = (char)usart_data_receive(uart_port); if (uart_index >= UART_BUFFER_SIZE) { uart_index = 0; } } else { break; } } if (RESET != usart_flag_get(uart_port, USART_FLAG_IDLE)) { usart_flag_clear(UART2, USART_FLAG_IDLE); if (uart_index > 0) rx_handler(); } sys_wakelock_release(LOCK_ID_USART); usart_interrupt_enable(uart_port, USART_INT_RBNE); } /*! \brief app uart config \param[in] none \retval none */ void app_uart_config(void) { rcu_periph_clock_disable(RCU_GPIOA); rcu_periph_clock_disable(RCU_GPIOB); rcu_periph_clock_disable(RCU_UART2); rcu_periph_clock_enable(RCU_GPIOA); rcu_periph_clock_enable(RCU_GPIOB); rcu_periph_clock_enable(RCU_UART2); gpio_af_set(UART2_TX_GPIO, UART2_TX_AF_NUM, UART2_TX_PIN); gpio_af_set(UART2_RX_GPIO, UART2_RX_AF_NUM, UART2_RX_PIN); gpio_mode_set(UART2_TX_GPIO, GPIO_MODE_AF, GPIO_PUPD_NONE, UART2_TX_PIN); gpio_output_options_set(UART2_TX_GPIO, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, UART2_TX_PIN); gpio_mode_set(UART2_RX_GPIO, GPIO_MODE_AF, GPIO_PUPD_NONE, UART2_RX_PIN); gpio_output_options_set(UART2_RX_GPIO, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, UART2_RX_PIN); /* close printf buffer */ setvbuf(stdout, NULL, _IONBF, 0); usart_deinit(UART2); usart_word_length_set(UART2, USART_WL_8BIT); usart_stop_bit_set(UART2, USART_STB_1BIT); usart_parity_config(UART2, USART_PM_NONE); usart_baudrate_set(UART2, 115200U); usart_receive_config(UART2, USART_RECEIVE_ENABLE); usart_transmit_config(UART2, USART_TRANSMIT_ENABLE); usart_interrupt_enable(UART2, USART_INT_RBNE); usart_receive_fifo_enable(UART2); usart_enable(UART2); /*wait IDLEF set and clear it*/ while(RESET == usart_flag_get(UART2, USART_FLAG_IDLE)) { } usart_flag_clear(UART2, USART_FLAG_IDLE); usart_interrupt_enable(UART2, USART_INT_IDLE); } /*! \brief app uart initialize \retval success 0 */ int app_uart_init(void) { if (sys_task_create_dynamic((const uint8_t *)"UART task", 512, UART_TASK_PRIORITY, app_uart_task, NULL) == NULL) { return -1; } if (sys_queue_init(&uart_queue, UART_QUEUE_SIZE, sizeof(struct uart_msg))) { return -2; } app_uart_config(); memset(uart_rx_buf, 0, UART_BUFFER_SIZE); uart_index = 0; cyclic_buf_init(&uart_cyc_buf, 4 * UART_BUFFER_SIZE); uart_irq_callback_register(UART2, app_uart_rx_irq_hdl); return 0; } 详细介绍代码
05-30
bool rrt_sharp(const Eigen::Vector3d &s, const Eigen::Vector3d &g) { ros::Time rrt_start_time = ros::Time::now(); bool goal_found = false; double c_square = (g - s).squaredNorm() / 4.0; /* kd tree init */ kdtree *kd_tree = kd_create(3); // Add start and goal nodes to kd tree kd_insert3(kd_tree, start_node_->x[0], start_node_->x[1], start_node_->x[2], start_node_); /* main loop */ int idx = 0; for (idx = 0; (ros::Time::now() - rrt_start_time).toSec() < search_time_ && valid_tree_node_nums_ < max_tree_node_nums_; ++idx) { /* biased random sampling */ Eigen::Vector3d x_rand; sampler_.samplingOnce(x_rand); // samplingOnce(x_rand); if (!map_ptr_->isStateValid(x_rand)) { continue; } struct kdres *p_nearest = kd_nearest3(kd_tree, x_rand[0], x_rand[1], x_rand[2]); if (p_nearest == nullptr) { ROS_ERROR("nearest query error"); continue; } RRTNode3DPtr nearest_node = (RRTNode3DPtr)kd_res_item_data(p_nearest); kd_res_free(p_nearest); Eigen::Vector3d x_new = steer(nearest_node->x, x_rand, steer_length_); if (!map_ptr_->isSegmentValid(nearest_node->x, x_new)) { continue; } /* 1. find parent */ /* kd_tree bounds search for parent */ Neighbour neighbour_nodes; neighbour_nodes.nearing_nodes.reserve(50); neighbour_nodes.center = x_new; struct kdres *nbr_set; nbr_set = kd_nearest_range3(kd_tree, x_new[0], x_new[1], x_new[2], search_radius_); if (nbr_set == nullptr) { ROS_ERROR("bkwd kd range query error"); break; } while (!kd_res_end(nbr_set)) { RRTNode3DPtr curr_node = (RRTNode3DPtr)kd_res_item_data(nbr_set); neighbour_nodes.nearing_nodes.emplace_back(curr_node, false, false); // store range query result so that we dont need to query again for rewire; kd_res_next(nbr_set); // go to next in kd tree range query result } kd_res_free(nbr_set); // reset kd tree range query /* choose parent from kd tree range query result*/ double dist2nearest = calDist(nearest_node->x, x_new); double min_dist_from_start(nearest_node->cost_from_start + dist2nearest); double cost_from_p(dist2nearest); RRTNode3DPtr min_node(nearest_node); // set the nearest_node as the default parent // TODO sort by potential cost-from-start for (auto &curr_node : neighbour_nodes.nearing_nodes) { if (curr_node.node_ptr == nearest_node) // the nearest_node already calculated and checked collision free { continue; } // check potential first, then check edge collision double curr_dist = calDist(curr_node.node_ptr->x, x_new); double potential_dist_from_start = curr_node.node_ptr->cost_from_start + curr_dist; if (min_dist_from_start > potential_dist_from_start) { bool connected = map_ptr_->isSegmentValid(curr_node.node_ptr->x, x_new); curr_node.is_checked = true; if (connected) { curr_node.is_valid = true; cost_from_p = curr_dist; min_dist_from_start = potential_dist_from_start; min_node = curr_node.node_ptr; } } } /* parent found within radius, then add a node to rrt and kd_tree */ // sample rejection double dist_to_goal = calDist(x_new, goal_node_->x); if (min_dist_from_start + dist_to_goal >= goal_node_->cost_from_start) { // ROS_WARN("parent found but sample rejected"); continue; } /* 1.1 add the randomly sampled node to rrt_tree */ RRTNode3DPtr new_node(nullptr); new_node = addTreeNode(min_node, x_new, min_dist_from_start, cost_from_p, dist_to_goal); /* 1.2 add the randomly sampled node to kd_tree */ kd_insert3(kd_tree, x_new[0], x_new[1], x_new[2], new_node); // end of find parent /* 2. try to connect to goal if possible */ if (dist_to_goal <= search_radius_) { bool is_connected2goal = map_ptr_->isSegmentValid(x_new, goal_node_->x); if (is_connected2goal && goal_node_->cost_from_start > dist_to_goal + new_node->cost_from_start) // a better path found { if (!goal_found) { first_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec(); } goal_found = true; changeNodeParent(goal_node_, new_node, dist_to_goal); vector<Eigen::Vector3d> curr_best_path; fillPath(goal_node_, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec()); // vis_ptr_->visualize_path(curr_best_path, "rrt_sharp_final_path"); // vis_ptr_->visualize_pointcloud(curr_best_path, "rrt_sharp_final_wpts"); if (use_informed_sampling_) { scale_[0] = goal_node_->cost_from_start / 2.0; scale_[1] = sqrt(scale_[0] * scale_[0] - c_square); scale_[2] = scale_[1]; sampler_.setInformedSacling(scale_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans_, scale_, rot_); vis_ptr_->visualize_ellipsoids(ellps, "informed_set", visualization::yellow, 0.2); } else if (use_GUILD_sampling_) { RRTNode3DPtr beacon_node = beaconSelect(); calInformedSet(beacon_node->cost_from_start, start_node_->x, beacon_node->x, scale1_, trans1_, rot1_); calInformedSet(goal_node_->cost_from_start - beacon_node->cost_from_start, beacon_node->x, goal_node_->x, scale2_, trans2_, rot2_); sampler_.setGUILDInformed(scale1_, trans1_, rot1_, scale2_, trans2_, rot2_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans1_, scale1_, rot1_); ellps.emplace_back(trans2_, scale2_, rot2_); vis_ptr_->visualize_ellipsoids(ellps, "local_set", visualization::green, 0.2); } } } /* 3.rewire */ std::priority_queue<RRTNode3DPtr, RRTNode3DPtrVector, RRTNodeComparator> rewire_queue; for (auto &curr_node : neighbour_nodes.nearing_nodes) { double dist_to_potential_child = calDist(new_node->x, curr_node.node_ptr->x); bool not_consistent = new_node->cost_from_start + dist_to_potential_child < curr_node.node_ptr->cost_from_start ? 1 : 0; bool promising = new_node->cost_from_start + dist_to_potential_child + curr_node.node_ptr->heuristic_to_goal < goal_node_->cost_from_start ? 1 : 0; if (not_consistent && promising) { bool connected(false); if (curr_node.is_checked) connected = curr_node.is_valid; else connected = map_ptr_->isSegmentValid(new_node->x, curr_node.node_ptr->x); // If we can get to a node via the sampled_node faster than via it's existing parent then change the parent if (connected) { double best_cost_before_rewire = goal_node_->cost_from_start; changeNodeParent(curr_node.node_ptr, new_node, dist_to_potential_child); rewire_queue.emplace(curr_node.node_ptr); if (best_cost_before_rewire > goal_node_->cost_from_start) { vector<Eigen::Vector3d> curr_best_path; fillPath(goal_node_, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec()); // vis_ptr_->visualize_path(curr_best_path, "rrt_sharp_final_path"); // vis_ptr_->visualize_pointcloud(curr_best_path, "rrt_sharp_final_wpts"); if (use_informed_sampling_) { scale_[0] = goal_node_->cost_from_start / 2.0; scale_[1] = sqrt(scale_[0] * scale_[0] - c_square); scale_[2] = scale_[1]; sampler_.setInformedSacling(scale_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans_, scale_, rot_); vis_ptr_->visualize_ellipsoids(ellps, "informed_set", visualization::yellow, 0.2); } else if (use_GUILD_sampling_) { RRTNode3DPtr beacon_node = beaconSelect(); calInformedSet(beacon_node->cost_from_start, start_node_->x, beacon_node->x, scale1_, trans1_, rot1_); calInformedSet(goal_node_->cost_from_start - beacon_node->cost_from_start, beacon_node->x, goal_node_->x, scale2_, trans2_, rot2_); sampler_.setGUILDInformed(scale1_, trans1_, rot1_, scale2_, trans2_, rot2_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans1_, scale1_, rot1_); ellps.emplace_back(trans2_, scale2_, rot2_); vis_ptr_->visualize_ellipsoids(ellps, "local_set", visualization::green, 0.2); } } } } /* go to the next entry */ } while (!rewire_queue.empty()) { RRTNode3DPtr curr_rewire_node = rewire_queue.top(); std::make_heap(const_cast<RRTNode3DPtr *>(&rewire_queue.top()), const_cast<RRTNode3DPtr *>(&rewire_queue.top()) + rewire_queue.size(), RRTNodeComparator()); // re-order the queue every time before pop a node, since the key may change during changeNodeParent() rewire_queue.pop(); struct kdres *nbr_set; nbr_set = kd_nearest_range3(kd_tree, curr_rewire_node->x[0], curr_rewire_node->x[1], curr_rewire_node->x[2], search_radius_); if (nbr_set == nullptr) { ROS_ERROR("bkwd kd range query error"); break; } while (!kd_res_end(nbr_set)) { RRTNode3DPtr curr_node = (RRTNode3DPtr)kd_res_item_data(nbr_set); double dist_to_potential_child = calDist(curr_rewire_node->x, curr_node->x); bool not_consistent = curr_rewire_node->cost_from_start + dist_to_potential_child < curr_node->cost_from_start ? 1 : 0; bool promising = curr_rewire_node->cost_from_start + dist_to_potential_child + curr_node->heuristic_to_goal < goal_node_->cost_from_start ? 1 : 0; if (not_consistent && promising) // If we can get to a node via the curr_rewire_node faster than via it's existing parent then change the parent { bool connected = map_ptr_->isSegmentValid(curr_rewire_node->x, curr_node->x); // If we can get to a node via the sampled_node faster than via it's existing parent then change the parent if (connected) { double best_cost_before_rewire = goal_node_->cost_from_start; changeNodeParent(curr_node, curr_rewire_node, dist_to_potential_child); rewire_queue.emplace(curr_node); if (best_cost_before_rewire > goal_node_->cost_from_start) { vector<Eigen::Vector3d> curr_best_path; fillPath(goal_node_, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec()); vis_ptr_->visualize_path(curr_best_path, "rrt_sharp_final_path"); vis_ptr_->visualize_pointcloud(curr_best_path, "rrt_sharp_final_wpts"); } } } kd_res_next(nbr_set); // go to next in kd tree range query result } kd_res_free(nbr_set); // reset kd tree range query } /* end of rewire */ } /* end of sample once */ vector<Eigen::Vector3d> vertice; vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> edges; sampleWholeTree(start_node_, vertice, edges); std::vector<visualization::BALL> balls; balls.reserve(vertice.size()); visualization::BALL node_p; node_p.radius = 0.2; for (size_t i = 0; i < vertice.size(); ++i) { node_p.center = vertice[i]; balls.push_back(node_p); } vis_ptr_->visualize_balls(balls, "tree_vertice", visualization::Color::blue, 1.0); vis_ptr_->visualize_pairline(edges, "tree_edges", visualization::Color::red, 0.1); if (goal_found) { final_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec(); fillPath(goal_node_, final_path_); ROS_INFO_STREAM("[RRT#]: first_path_use_time: " << first_path_use_time_ << ", length: " << solution_cost_time_pair_list_.front().first); } else if (valid_tree_node_nums_ == max_tree_node_nums_) { ROS_ERROR_STREAM("[RRT#]: NOT CONNECTED TO GOAL after " << max_tree_node_nums_ << " nodes added to rrt-tree"); } else { ROS_ERROR_STREAM("[RRT#]: NOT CONNECTED TO GOAL after " << (ros::Time::now() - rrt_start_time).toSec() << " seconds"); } return goal_found; } 注释上述代码
最新发布
09-19
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值