if (std::isnan(hw_positions_[i]))

时间: 2025-08-13 18:22:40 AIGC 浏览: 17
<think>好的,我现在需要帮助用户检查他们在C++代码中使用std::isnan的条件语句是否正确,以及该功能是否正常。首先,用户提到了一个具体的函数:CaffeWeightFactory::checkForNans,其中有一个模板函数,接受void指针、元素数量和层名作为参数。函数内部将指针转换为类型T的指针,然后遍历每个元素,使用std::isnan(float(v[i]))进行检查。 首先,我应该回顾一下std::isnan的正确用法。根据C++标准,std::isnan用于检测一个浮点数是否为NaN(Not a Number)。它的参数应该是浮点类型,比如float、double或者long double。如果参数是整数类型,可能会因为转换问题导致不正确的检测结果,因为NaN只存在于浮点数中。 接下来,我需要分析用户提供的代码段。在代码中,模板参数T可能是任意类型,但函数内部将v[i]转换为float后再传给std::isnan。这里可能存在潜在的问题。例如,如果T原本是double类型,转换为float可能会导致精度损失,但转换为float后的NaN是否仍然被正确检测呢?不过,NaN在经过转换后通常仍然会是NaN,所以这里可能没问题。但需要考虑T是否为整数类型的情况。如果T是int或其他整数类型,转换为float后,整数不可能是NaN,所以这样的检查就没有意义,甚至可能引入错误。因此,模板函数的设计可能需要限制T为浮点类型,或者确保只在T为浮点类型时调用该函数。 另一个需要注意的地方是,当T本身就是float时,直接使用std::isnan(v[i])即可,不需要强制转换为float,因为这样转换是多余的。但如果T是其他类型比如double,转换为float可能会导致NaN变成非NaN,或者非NaN变成NaN吗?一般来说,如果double的NaN在转换为float时,应该保持为NaN,所以std::isnan(float(v[i]))在这种情况下仍然有效。但需要确认C++标准中的转换规则。 此外,用户提到的函数是用于检查权重中的NaN值,这在深度学习中处理模型权重时很常见。因此,确保所有浮点类型的权重都被正确检查是非常重要的。如果T是半精度浮点(比如FP16),转换为float可能会更安全,因为std::isnan不支持半精度类型,但转换为float后可以正确检测。 总结一下,代码中的条件语句是否正确取决于模板参数T的实际类型。如果T是浮点类型(如float、double),那么转换为float后再检查是可行的,尽管可能有精度损失。但如果T是非浮点类型,这样的检查是没有意义的。因此,建议在模板函数中添加静态断言,确保T只能是浮点类型,或者在调用该函数时确保传入的是浮点数据。 另外,代码中将v[i]转换为float再检查NaN,这可能在处理double类型时不够精确,因为某些特定的NaN模式可能在转换后丢失。更准确的做法是根据T的实际类型来调用相应的std::isnan重载,或者使用C++17的if constexpr进行类型分派,避免不必要的转换。 用户的问题还提到检查hw_positions_数组,这可能是一个存储硬件位置的数组,类型可能是浮点数。因此,代码中使用std::isnan来检查数组中的每个元素是否为NaN是正确的,前提是数组元素确实是浮点类型。 最后,需要确保编译环境支持C++11或更高标准,因为std::isnan在C++11中引入,并且对于不同的浮点类型有正确的重载。如果代码在旧的标准下编译,可能会导致问题。 根据这些分析,我需要向用户指出代码中的潜在问题,并提供改进建议,比如添加类型检查、避免不必要的转换,或者使用更合适的类型分派方法。</think>### 检查`std::isnan`在条件语句中的正确性 您提供的代码段中,`std::isnan`用于检测浮点数值是否为`NaN`(非数字)。以下是关键分析点: #### 1. **代码逻辑分析** ```cpp template <typename T> bool CaffeWeightFactory::checkForNans(const void* values, int count, const std::string& layerName) { const T* v = reinterpret_cast<const T*>(values); for (int i = 0; i < count; i++) { if (std::isnan(float(v[i]))) { // 强制转换为float后检查NaN // 处理逻辑 return false; } } return true; } ``` - **功能**:遍历数组`v`,检查每个元素是否为`NaN`。 - **潜在问题**: - **类型转换风险**:若模板参数`T`实际为`double`或更高精度类型,强制转换为`float`可能导致精度丢失,但不会影响`NaN`的判断[^1]。 - **适用性限制**:若`T`为非浮点类型(如`int`),`v[i]`转换为`float`后不可能为`NaN`,此时检查无意义。 #### 2. **改进建议** - **类型安全增强**: ```cpp // 使用C++17的if constexpr进行类型分派 if constexpr (std::is_floating_point_v<T>) { if (std::isnan(v[i])) { // 直接检查原始类型 return false; } } else { // 非浮点类型无需检查NaN } ``` - **静态断言限制类型**: ```cpp static_assert(std::is_floating_point_v<T>, "T must be a floating-point type"); ``` #### 3. **实际场景验证** - **浮点类型验证**: - `float`和`double`类型的`NaN`均能被`std::isnan`正确识别[^1]。 - 若`hw_positions_`数组为`float`类型,原始代码逻辑正确。 - **错误场景示例**: - 若`T`为`int`,`std::isnan(float(v[i]))`始终为`false`,可能导致漏检(若`v[i]`本应为浮点类型但被误传为整数)。 --- ### 总结 1. **当前代码的正确性**: - **正确**:当`T`为`float`时,`std::isnan(float(v[i]))`有效。 - **风险**:若`T`为`double`,转换可能丢失部分信息;若`T`为非浮点类型,逻辑冗余。 2. **优化方向**: - 添加类型限制(如`static_assert`)确保模板参数为浮点类型。 - 直接使用原始类型检查(避免强制转换)。
阅读全文

相关推荐

#include <iostream> #include <vector> #include <string> #include <cstring> #include <cmath> #include <algorithm> #include <onnxruntime_cxx_api.h> // 检查浮点数有效性 bool is_valid_float(float value) { return !std::isnan(value) && std::isfinite(value); } int main() { try { // 1. 初始化ONNX Runtime环境 //Ort::Env env(ORT_LOGGING_LEVEL_WARNING, "ONNX-Runtime"); Ort::Env env(ORT_LOGGING_LEVEL_VERBOSE, "ONNX-Runtime"); Ort::SessionOptions session_options; // 2. 加载ONNX模型 Ort::Session session(env, L"model.onnx", session_options); Ort::AllocatorWithDefaultOptions allocator; #if 0 auto model_metadata = session.GetModelMetadata(); auto pModelProducer = model_metadata.GetProducerNameAllocated(allocator); std::cout << "Model producer: " << pModelProducer.get() << "\n"; // 获取所有权重名称 size_t num_nodes = session.GetOverridableInitializerCount(); for (size_t i = 0; i < num_nodes; i++) { auto name_ptr = session.GetOverridableInitializerNameAllocated(i, allocator); std::cout << "Override name: " << *name_ptr << "\n"; } #endif // 3. 获取模型输入信息 size_t num_inputs = session.GetInputCount(); std::vector<std::string> input_names; std::vector<std::vector<int64_t>> input_shapes; std::vector<ONNXTensorElementDataType> input_types; std::cout << "===== Model Input Analysis =====\n"; for (size_t i = 0; i < num_inputs; i++) { auto name_ptr = session.GetInputNameAllocated(i, allocator); input_names.push_back(name_ptr.get()); auto type_info = session.GetInputTypeInfo(i); auto tensor_info = type_info.GetTensorTypeAndShapeInfo(); input_shapes.push_back(tensor_info.GetShape()); input_types.push_back(tensor_info.GetElementType()); std::cout << "Input [" << i << "]: " << input_names.back() << " | Type: " << input_types.back() << " | Shape: ["; for (auto dim : input_shapes.back()) { std::cout << dim << (dim == -1 ? "(dynamic)" : "") << ", "; } std::cout << "]\n"; } // 4. 获取模型输出信息 size_t num_outputs = session.GetOutputCount(); std::vector<std::string> output_names; std::vector<std::vector<int64_t>> output_shapes; std::cout << "\n===== Model Output Analysis =====\n"; for (size_t i = 0; i < num_outputs; i++) { auto name_ptr = session.GetOutputNameAllocated(i, allocator); output_names.push_back(name_ptr.get()); auto type_info = session.GetOutputTypeInfo(i); auto tensor_info = type_info.GetTensorTypeAndShapeInfo(); output_shapes.push_back(tensor_info.GetShape()); std::cout << "Output [" << i << "]: " << output_names.back() << " | Type: " << tensor_info.GetElementType() << " | Shape: ["; for (auto dim : output_shapes.back()) { std::cout << dim << (dim == -1 ? "(dynamic)" : "") << ", "; } std::cout << "]\n"; } // 5. 准备输入数据 //std::vector<int64_t> token_ids = { 35377 }; std::vector<int64_t> token_ids = { 100 }; //std::vector<int64_t> token_ids = { 70645 }; const size_t seq_length = token_ids.size(); const int64_t batch_size = 1; // 6. 动态调整输入形状 for (auto& shape : input_shapes) { for (size_t i = 0; i < shape.size(); i++) { if (shape[i] == -1) { if (i == 0) shape[i] = batch_size; // 第一维 = batch size else if (i == 1) shape[i] = seq_length; // 第二维 = 序列长度 else shape[i] = 1; // 其他维度设为1 } } } // 7. 创建输入张量 std::vector<Ort::Value> input_tensors; Ort::MemoryInfo memory_info = Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeDefault); std::cout << "\n===== Input Tensor Preparation =====\n"; for (size_t i = 0; i < num_inputs; i++) { const auto& name = input_names[i]; const auto& shape = input_shapes[i]; // 计算预期元素数量 size_t expected_elements = 1; for (auto dim : shape) expected_elements *= dim; std::cout << "Preparing input: " << name << " | Shape: ["; for (auto dim : shape) std::cout << dim << ", "; std::cout << "] | Elements: " << expected_elements << "\n"; if (name.find("input") != std::string::npos || name.find("ids") != std::string::npos) { if (shape.size() >= 2 && shape[0] == batch_size && shape[1] == seq_length) { input_tensors.emplace_back(Ort::Value::CreateTensor<int64_t>( memory_info, token_ids.data(), token_ids.size(), shape.data(), shape.size() )); std::cout << " Using token_ids data (size: " << token_ids.size() << ")\n"; } else { throw std::runtime_error("Input shape mismatch for token_ids"); } } else if (name.find("mask") != std::string::npos) { std::vector<int64_t> mask(seq_length, 1); input_tensors.emplace_back(Ort::Value::CreateTensor<int64_t>( memory_info, mask.data(), mask.size(), shape.data(), shape.size() )); std::cout << " Using attention_mask data\n"; } else if (name.find("type") != std::string::npos) { std::vector<int64_t> type_ids(seq_length, 0); input_tensors.emplace_back(Ort::Value::CreateTensor<int64_t>( memory_info, type_ids.data(), type_ids.size(), shape.data(), shape.size() )); std::cout << " Using token_type_ids data\n"; } else { throw std::runtime_error("Unsupported input type: " + name); } } // 8. 准备输入/输出名称指针 std::vector<const char*> input_names_ptr; for (const auto& name : input_names) input_names_ptr.push_back(name.c_str()); std::vector<const char*> output_names_ptr; for (const auto& name : output_names) output_names_ptr.push_back(name.c_str()); // 9. 运行模型推理 std::cout << "\n===== Running Inference =====\n"; auto output_tensors = session.Run( Ort::RunOptions{ nullptr }, input_names_ptr.data(), input_tensors.data(), input_tensors.size(), output_names_ptr.data(), output_names_ptr.size() ); // 10. 分析输出结果 std::cout << "\n===== Inference Results =====\n"; for (size_t i = 0; i < output_tensors.size(); i++) { auto& tensor = output_tensors[i]; if (!tensor.IsTensor()) { std::cerr << " Output [" << i << "] is not a tensor\n"; continue; } auto tensor_info = tensor.GetTensorTypeAndShapeInfo(); auto shape = tensor_info.GetShape(); size_t element_count = tensor_info.GetElementCount(); auto data_type = tensor_info.GetElementType(); std::cout << "Output [" << i << "]: " << output_names[i] << "\n"; std::cout << " Shape: ["; for (auto dim : shape) std::cout << dim << ", "; std::cout << "] | Elements: " << element_count << "\n"; // 关键诊断:检查输出维度 if (shape.size() >= 2 && shape[1] != seq_length) { std::cout << "WARNING: Output sequence length (" << shape[1] << ") does not match input length (" << seq_length << ")\n"; } // 详细输出分析 if (data_type == ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT) { double* data1 = tensor.GetTensorMutableData<double>(); float* data = tensor.GetTensorMutableData<float>(); // 统计有效值 int valid_count = 0; int nan_count = 0; float min_val = FLT_MAX, max_val = -FLT_MAX; for (size_t j = 0; j < element_count; j++) { if (is_valid_float(data[j])) { valid_count++; if (data[j] < min_val) min_val = data[j]; if (data[j] > max_val) max_val = data[j]; } else { nan_count++; } } std::cout << " Valid values: " << valid_count << "/" << element_count << "\n"; if (valid_count > 0) { std::cout << " Value range: [" << min_val << ", " << max_val << "]\n"; // 打印完整输出(元素少时) if (element_count <= 20) { std::cout << " Full output: "; for (size_t j = 0; j < element_count; j++) { std::cout << data[j] << " "; } std::cout << "\n"; } } } else { std::cout << " Data type: " << data_type << " (printing not supported)\n"; } std::cout << "\n"; } } catch (const Ort::Exception& e) { std::cerr << "ONNX Runtime Error: " << e.what() << "\n"; return 1; } catch (const std::exception& e) { std::cerr << "Error: " << e.what() << "\n"; return 1; } std::cout << "Inference completed. Press Enter to exit..."; getchar(); return 0; }这代码输入没问题,但是输出的向量是无效的float值

#ifndef ARS_RADAR_H_ #define ARS_RADAR_H_ #include <ros/ros.h> #include <string> #include <vector> #include <thread> //#include"gps_msgs/gpsUtm.h" #include <can_msgs/Object.h> #include <can_msgs/ObjectArray.h> #include <can_msgs/FrameArray.h> #include <jsk_recognition_msgs/BoundingBox.h> #include <jsk_recognition_msgs/BoundingBoxArray.h> #include #include #include <sensor_msgs/PointCloud2.h> #include <diagnostic_msgs/DiagnosticStatus.h> #include <cmath> #include <unordered_map> class ArsRadar { public: ArsRadar(); ~ArsRadar(){ pthread_rwlock_destroy(&egoSpeedLock); pthread_rwlock_destroy(&countLock); }; bool init(); void run(); private: void sendMsgToArs(const ros::TimerEvent&); void configArs(const ros::TimerEvent&); void canMsg_callback(const can_msgs::FrameArray::ConstPtr& msg); //void gps_callback(const gps_msgs::gpsUtm::ConstPtr& msg); void parse_msg(const can_msgs::Frame &frame, int index, int n); void pubBoundingBoxArray(); enum Cluster_DynProp { Target_Move = 0, //移动 Target_Static = 1, //静止 Target_Come = 2, //来向 Target_May_Static = 3, //可能静止 Target_Unknow = 4, //未知 Target_Across = 5, //横穿 Target_Across_Static = 6, //横穿静止 Target_Stop = 7 //停止 }; typedef struct _Info { std::string type; uint8_t r; uint8_t g; uint8_t b; _Info(const std::string& t, uint8_t _r, uint8_t _g, uint8_t _b) { type = t; r = _r; g = _g; b = _b; } }Info; std::vector<Info> Infos; private: ros::Subscriber sub_can_; // 订阅can分析仪消息 ros::Subscriber sub_gps_; ros::Publisher pub_can_; // 向can分析仪发布消息 ros::Publisher pub_can_config; // 向can分析仪发布配置消息 ros::Publisher pub_object_; // 发布离我最近的障碍物消息 ros::Publisher pub_objects_; // 发布所有障碍物消息 ros::Publisher pub_cloud_; // 发布点云消息 pcl::PointCloud cloud; pcl::PointXYZRGB point; sensor_msgs::PointCloud2 output; std::string from_can_topic_; std::string to_can_topic_; std::string gpsUtm_topic; can_msgs::ObjectArray ars_objects_; can_msgs::Object ars_object_; can_msgs::Object ars_object_car; bool is_sendMsgToArs_; ros::Timer timer_1, timer_2; std::unordered_map<int, int> MapObjectId2indexInObjectArray; double egoSpeed; // m/s double yawRate; // deg/s pthread_rwlock_t egoSpeedLock, countLock; int readCount; }; #endif 在此代码上推出上面引用的头文件中#include"gps_msgs/gpsUtm.h"的msgs文件

/* * Copyright 2018-2019 Autoware Foundation. All rights reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "ekf_localizer/ekf_localizer.h" // clang-format off #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl #define DEBUG_INFO(...) { if (show_debug_info_) { ROS_INFO(__VA_ARGS__); } } #define DEBUG_PRINT_MAT(X) { if (show_debug_info_) { std::cout << #X << ": " << X << std::endl; } } // clang-format on /* x, y:机器人位置。 yaw:机器人朝向(偏航角)。 yaw_bias:偏航角偏差(用于估计传感器误差)。 vx, wz:线速度和角速度。 */ EKFLocalizer::EKFLocalizer() : nh_(""), pnh_("~"), dim_x_(8 /* x, y, yaw, yaw_bias, vx, wz */) { pnh_.param("show_debug_info", show_debug_info_, bool(false)); // 是否显示调试信息 pnh_.param("predict_frequency", ekf_rate_, double(50.0)); // EKF 预测频率(Hz) ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); // 计算时间步长(秒) pnh_.param("enable_yaw_bias_estimation", enable_yaw_bias_estimation_, bool(true)); // 是否估计偏航角偏差 pnh_.param("extend_state_step", extend_state_step_, int(50)); // 状态扩展步数(用于滑动窗口优化) pnh_.param("pose_frame_id", pose_frame_id_, std::string("map")); // 输出位姿的坐标系 ID pnh_.param("output_frame_id", output_frame_id_, std::string("base_link")); // 输出坐标系 /* pose measurement 位姿测量参数*/ pnh_.param("pose_additional_delay", pose_additional_delay_, double(0.0)); // 额外延迟(秒) pnh_.param("pose_measure_uncertainty_time", pose_measure_uncertainty_time_, double(0.01)); // 测量不确定性时间 pnh_.param("pose_rate", pose_rate_, double(10.0)); // 位姿测量频率(用于协方差计算) // used for covariance calculation pnh_.param("pose_gate_dist", pose_gate_dist_, double(10000.0)); // 马氏距离阈值(异常值过滤) // Mahalanobis limit pnh_.param("pose_stddev_x", pose_stddev_x_, double(0.05)); // X 方向标准差(米) pnh_.param("pose_stddev_y", pose_stddev_y_, double(0.05)); // Y 方向标准差(米) pnh_.param("pose_stddev_yaw", pose_stddev_yaw_, double(0.035)); // 偏航角标准差(弧度) pnh_.param("use_pose_with_covariance", use_pose_with_covariance_, bool(false)); // 是否使用带协方差的位姿输入 /* twist measurement 速度测量参数*/ pnh_.param("twist_additional_delay", twist_additional_delay_, double(0.0)); // 额外延迟(秒) pnh_.param("twist_rate", twist_rate_, double(10.0)); // 速度测量频率(用于协方差计算) // used for covariance calculation pnh_.param("twist_gate_dist", twist_gate_dist_, double(10000.0)); // 马氏距离阈值(异常值过滤) // Mahalanobis limit pnh_.param("twist_stddev_vx", twist_stddev_vx_, double(0.2)); // 线速度标准差(米/秒) pnh_.param("twist_stddev_wz", twist_stddev_wz_, double(0.03)); // 角速度标准差(弧度/秒) pnh_.param("use_twist_with_covariance", use_twist_with_covariance_, bool(false)); // 是否使用带协方差的速度输入 /* IMU measurement parameters */ pnh_.param("use_imu", use_imu_, bool(true)); pnh_.param("imu_rate", imu_rate_, double(50.0)); pnh_.param("imu_gate_dist", imu_gate_dist_, double(10000.0)); pnh_.param("imu_stddev_ax", imu_stddev_ax_, double(0.5)); pnh_.param("imu_stddev_wz", imu_stddev_wz_, double(0.01)); /* process noise 过程噪声参数*/ double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c; double proc_stddev_ax_c, proc_stddev_wz_imu_c; pnh_.param("proc_stddev_yaw_c", proc_stddev_yaw_c, double(0.005)); // 偏航角过程噪声(连续时间) pnh_.param("proc_stddev_yaw_bias_c", proc_stddev_yaw_bias_c, double(0.001)); // 偏航角偏差过程噪声 pnh_.param("proc_stddev_vx_c", proc_stddev_vx_c, double(2.0)); // 线速度过程噪声 pnh_.param("proc_stddev_wz_c", proc_stddev_wz_c, double(0.2)); // 角速度过程噪声 if (!enable_yaw_bias_estimation_) { proc_stddev_yaw_bias_c = 0.0; } /* convert to continuous to discrete 转换为离散时间噪声(乘以时间步长)*/ proc_cov_vx_d_ = std::pow(proc_stddev_vx_c, 2.0) * ekf_dt_; proc_cov_wz_d_ = std::pow(proc_stddev_wz_c, 2.0) * ekf_dt_; proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c, 2.0) * ekf_dt_; proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c, 2.0) * ekf_dt_; proc_cov_ax_d_ = std::pow(proc_stddev_ax_c, 2.0) * ekf_dt_; proc_cov_wz_imu_d_ = std::pow(proc_stddev_wz_imu_c, 2.0) * ekf_dt_; /* initialize ros system */ // 定时器(用于 EKF 预测步) timer_control_ = nh_.createTimer(ros::Duration(ekf_dt_), &EKFLocalizer::timerCallback, this); // 发布话题 //pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ekf_pose", 1); pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 10); pub_pose_cov_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("ekf_pose_with_covariance", 10); //pub_twist_ = nh_.advertise<geometry_msgs::TwistStamped>("/ekf_twist", 1); pub_twist_ = nh_.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 10); pub_twist_cov_ = nh_.advertise<geometry_msgs::TwistWithCovarianceStamped>("ekf_twist_with_covariance", 10); pub_yaw_bias_ = pnh_.advertise<std_msgs::Float64>("estimated_yaw_bias", 10); // 订阅话题 sub_initialpose_ = nh_.subscribe("initialpose", 10, &EKFLocalizer::callbackInitialPose, this); sub_pose_with_cov_ = nh_.subscribe("in_pose_with_covariance", 10, &EKFLocalizer::callbackPoseWithCovariance, this); sub_pose_ = nh_.subscribe("/in_pose", 10, &EKFLocalizer::callbackPose, this); sub_twist_with_cov_ = nh_.subscribe("in_twist_with_covariance", 10, &EKFLocalizer::callbackTwistWithCovariance, this); //sub_twist_ = nh_.subscribe("/can_info", 10, &EKFLocalizer::callbackTwist, this); imu_sub_.subscribe(nh_, "/imu_raw", 100); vehicle_sub_.subscribe(nh_, "/can_info", 50); sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10)); sync_->connectInput(imu_sub_, vehicle_sub_); sync_->registerCallback(boost::bind(&EKFLocalizer::sensorCallback, this, _1, _2)); sync_->setMaxIntervalDuration(ros::Duration(0.003)); // 3ms容差 dim_x_ex_ = dim_x_ * extend_state_step_; // 扩展状态维度(用于滑动窗口优化) initEKF(); // 初始化 EKF 内部状态 last_timer_call_time_ = 0.0; /* debug */ pub_debug_ = pnh_.advertise<std_msgs::Float64MultiArray>("debug", 1); // 调试信息(数组) pub_measured_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("debug/measured_pose", 1); // 调试用测量位姿 pub_measured_imu_ = pnh_.advertise<sensor_msgs::Imu>("debug/measured_imu", 1); }; EKFLocalizer::~EKFLocalizer(){}; /* * timerCallback */ void EKFLocalizer::timerCallback(const ros::TimerEvent& e) { DEBUG_INFO("========================= timer called ========================="); /* predict model in EKF 预测步(Predict)*/ auto start = std::chrono::system_clock::now(); DEBUG_INFO("------------------------- start prediction -------------------------"); double actual_dt = (last_timer_call_time_ > 0.0) ? (ros::Time::now().toSec() - last_timer_call_time_) : ekf_dt_; predictKinematicsModel(actual_dt); // 执行运动模型预测 double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); // 计算耗时 DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end prediction -------------------------\n"); /* pose measurement update */ if (current_pose_ptr_ != nullptr) // 位姿更新(当有新位姿数据时) { DEBUG_INFO("------------------------- start Pose -------------------------"); start = std::chrono::system_clock::now(); measurementUpdatePose(*current_pose_ptr_); // 融合传感器位姿数据 elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end Pose -------------------------\n"); } /* twist measurement update */ if (current_twist_ptr_ != nullptr) // 速度更新(当有新速度数据时) { DEBUG_INFO("------------------------- start twist -------------------------"); start = std::chrono::system_clock::now(); measurementUpdateTwist(*current_twist_ptr_); // 融合速度数据 elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end twist -------------------------\n"); } /* IMU measurement update */ if (use_imu_ && current_imu_ptr_ != nullptr) { DEBUG_INFO("------------------------- start IMU -------------------------"); start = std::chrono::system_clock::now(); measurementUpdateIMU(*current_imu_ptr_); elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); DEBUG_INFO("[EKF] measurementUpdateIMU calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end IMU -------------------------\n"); } /* set current pose, twist */ setCurrentResult(); // 更新内部状态 last_timer_call_time_ = ros::Time::now().toSec(); /* publish ekf result */ publishEstimateResult(); // 发布最终估计结果 } void EKFLocalizer::showCurrentX() { // 检查调试信息显示标志是否开启 if (show_debug_info_) { // 创建临时矩阵存储状态向量 Eigen::MatrixXd X(dim_x_, 1); // 从EKF获取最新状态估计值 ekf_.getLatestX(X); // 打印转置后的状态向量(行向量形式) DEBUG_PRINT_MAT(X.transpose()); } } /* * setCurrentResult */ void EKFLocalizer::setCurrentResult() { current_ekf_pose_.header.frame_id = pose_frame_id_; current_ekf_pose_.header.stamp = ros::Time::now(); current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X); current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y); tf2::Quaternion q_tf; double roll, pitch, yaw; if (current_pose_ptr_ != nullptr) { current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z; tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */ tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw); } else { current_ekf_pose_.pose.position.z = 0.0; roll = 0; pitch = 0; } yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB); q_tf.setRPY(roll, pitch, yaw); tf2::convert(q_tf, current_ekf_pose_.pose.orientation); current_ekf_twist_.header.frame_id = output_frame_id_; current_ekf_twist_.header.stamp = ros::Time::now(); current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX); current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ) + ekf_.getXelement(IDX::WZ_IMU); } /* * broadcastTF */ void EKFLocalizer::broadcastTF(ros::Time time) { // if (current_ekf_pose_.header.frame_id == "") // { // return; // } // tf::Transform transform; // transform.setOrigin(tf::Vector3(current_ekf_pose_.pose.position.x, current_ekf_pose_.pose.position.y, current_ekf_pose_.pose.position.z)); // tf::Quaternion current_q( // current_ekf_pose_.pose.orientation.x, // current_ekf_pose_.pose.orientation.y, // current_ekf_pose_.pose.orientation.z, // current_ekf_pose_.pose.orientation.w // ); // transform.setRotation(current_q); // tf_br_.sendTransform(tf::StampedTransform(transform, time, "/map", output_frame_id_)); if (current_ekf_pose_.header.frame_id == "") { return; } geometry_msgs::TransformStamped transformStamped; transformStamped.header = current_ekf_pose_.header; transformStamped.child_frame_id = output_frame_id_; transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x; transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y; transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z; transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x; transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y; transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z; transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w; tf_br_.sendTransform(transformStamped); } /* * getTransformFromTF */ bool EKFLocalizer::getTransformFromTF(std::string parent_frame, std::string child_frame, geometry_msgs::TransformStamped& transform) { // tf::TransformListener listener; // for (int i = 0; i < 50; ++i) // { // try // { // auto now = ros::Time(0); // listener.waitForTransform(parent_frame, child_frame, now, ros::Duration(10.0)); // listener.lookupTransform(parent_frame, child_frame, now, transform); // return true; // } // catch (tf::TransformException& ex) // { // ROS_ERROR("%s", ex.what()); // ros::Duration(0.1).sleep(); // } // } // return false; tf2_ros::Buffer tf_buffer; tf2_ros::TransformListener tf_listener(tf_buffer); ros::Duration(0.1).sleep(); if (parent_frame.front() == '/') parent_frame.erase(0, 1); if (child_frame.front() == '/') child_frame.erase(0, 1); for (int i = 0; i < 50; ++i) { try { transform = tf_buffer.lookupTransform(parent_frame, child_frame, ros::Time(0)); return true; } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); } } return false; } /* * callbackInitialPose */ void EKFLocalizer::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped& initialpose) { // (1) 获取 TF 变换 // tf::StampedTransform transform; // if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform)) // { // ROS_ERROR("[EKF] TF transform failed. parent = %s, child = %s", // pose_frame_id_.c_str(), initialpose.header.frame_id.c_str()); // return; // 必须返回,避免使用无效变换 // } // // (2) 初始化状态向量 X // Eigen::MatrixXd X(dim_x_, 1); // X.setZero(); // 显式初始化所有状态为 0 // // 将 initialpose 变换到 pose_frame_id_ 坐标系 // tf::Pose tf_initial_pose; // tf::poseMsgToTF(initialpose.pose.pose, tf_initial_pose); // tf::Pose transformed_pose = transform * tf_initial_pose; // 正确应用 TF 变换 // X(IDX::X) = transformed_pose.getOrigin().x(); // X(IDX::Y) = transformed_pose.getOrigin().y(); // X(IDX::YAW) = tf::getYaw(transformed_pose.getRotation()); // X(IDX::YAWB) = 0.0; // 偏航角偏差初始化为 0 // X(IDX::VX) = 0.0; // 速度初始化为 0 // X(IDX::WZ) = 0.0; // 角速度初始化为 0 // X(IDX::AX) = 0.0; // 加速度初始化为 0 // X(IDX::WZ_IMU) = 0.0; // IMU 角速度初始化为 0 // // (3) 初始化协方差矩阵 P // Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); // const auto& cov = initialpose.pose.covariance; // // 检查协方差矩阵是否有效(非负且非全零) // if (cov[0] > 0.0) P(IDX::X, IDX::X) = cov[0]; // X variance // if (cov[7] > 0.0) P(IDX::Y, IDX::Y) = cov[7]; // Y variance // if (cov[35] > 0.0) P(IDX::YAW, IDX::YAW) = cov[35]; // YAW variance // // 其他状态的协方差(默认值) // P(IDX::YAWB, IDX::YAWB) = 0.0001; // 偏航角偏差 // P(IDX::VX, IDX::VX) = 0.01; // 速度 // P(IDX::WZ, IDX::WZ) = 0.01; // 角速度 // P(IDX::AX, IDX::AX) = 0.01; // 加速度 // P(IDX::WZ_IMU, IDX::WZ_IMU) = 0.01; // IMU 角速度 // // (4) 初始化 EKF // ekf_.init(X, P, extend_state_step_); geometry_msgs::TransformStamped transform; if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform)) { ROS_ERROR("[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(), initialpose.header.frame_id.c_str()); }; Eigen::MatrixXd X(dim_x_, 1); Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); X(IDX::X) = initialpose.pose.pose.position.x /* + transform.transform.translation.x */; X(IDX::Y) = initialpose.pose.pose.position.y /* + transform.transform.translation.y */; X(IDX::YAW) = tf2::getYaw(initialpose.pose.pose.orientation) /* + tf2::getYaw(transform.transform.rotation) */; X(IDX::YAWB) = 0.0; X(IDX::VX) = 0.0; X(IDX::WZ) = 0.0; X(IDX::AX) = 0.0; // 加速度初始化为 0 X(IDX::WZ_IMU) = 0.0; // IMU 角速度初始化为 0 P(IDX::X, IDX::X) = initialpose.pose.covariance[0]; P(IDX::Y, IDX::Y) = initialpose.pose.covariance[6 + 1]; P(IDX::YAW, IDX::YAW) = initialpose.pose.covariance[6 * 5 + 5]; P(IDX::YAWB, IDX::YAWB) = 0.0001; P(IDX::VX, IDX::VX) = 0.01; P(IDX::WZ, IDX::WZ) = 0.01; P(IDX::AX, IDX::AX) = 0.01; // 加速度 P(IDX::WZ_IMU, IDX::WZ_IMU) = 0.01; // IMU 角速度 ekf_.init(X, P, extend_state_step_); }; /* * callbackPose */ void EKFLocalizer::callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg) { if (!use_pose_with_covariance_) { current_pose_ptr_ = std::make_shared<geometry_msgs::PoseStamped>(*msg); } }; /* * callbackPoseWithCovariance */ void EKFLocalizer::callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) { if (use_pose_with_covariance_) { geometry_msgs::PoseStamped pose; pose.header = msg->header; pose.pose = msg->pose.pose; current_pose_ptr_ = std::make_shared<geometry_msgs::PoseStamped>(pose); current_pose_covariance_ = msg->pose.covariance; } }; /* * callbackTwist */ void EKFLocalizer::sensorCallback(const sensor_msgs::Imu::ConstPtr& imu_msg, const autoware_can_msgs::CANInfo::ConstPtr& vehicle_msg) { geometry_msgs::TwistStamped twist_msg; twist_msg.header = vehicle_msg->header; twist_msg.header.frame_id = "/base_link"; // 根据实际坐标系设置 // 设置线速度 (来自CAN) twist_msg.twist.linear.x = (vehicle_msg->speed / 3.6) * cos(vehicle_msg->angle); twist_msg.twist.linear.y = 0.0; twist_msg.twist.linear.z = 0.0; // 设置角速度 (来自IMU) //twist_msg.twist.angular = imu_msg->angular_velocity; // 计算运动学模型的角速度 twist_msg.twist.angular.x = 0; twist_msg.twist.angular.y = 0; twist_msg.twist.angular.z = ((vehicle_msg->speed / 3.6)*sin(vehicle_msg->angle))/1.137; current_twist_ptr_ = std::make_shared<geometry_msgs::TwistStamped>(twist_msg); if (imu_msg->header.frame_id != "imu") { ROS_WARN_DELAYED_THROTTLE(2, "IMU frame_id is %s, but expected 'imu'", imu_msg->header.frame_id.c_str()); } current_imu_ptr_ = std::make_shared<sensor_msgs::Imu>(*imu_msg); // // 估计IMU零偏 // estimateIMUBias(); } /* * callbackTwistWithCovariance */ void EKFLocalizer::callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg) { if (use_twist_with_covariance_) { geometry_msgs::TwistStamped twist; twist.header = msg->header; twist.twist = msg->twist.twist; current_twist_ptr_ = std::make_shared<geometry_msgs::TwistStamped>(twist); current_twist_covariance_ = msg->twist.covariance; } }; /* * initEKF */ void EKFLocalizer::initEKF() { Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y P(IDX::YAW, IDX::YAW) = 50.0; // for yaw P(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias P(IDX::VX, IDX::VX) = 1000.0; // for vx P(IDX::WZ, IDX::WZ) = 50.0; // for wz P(IDX::AX, IDX::AX) = 10.0; P(IDX::WZ_IMU, IDX::WZ_IMU) = 1.0; ekf_.init(X, P, extend_state_step_); } /* * predictKinematicsModel */ void EKFLocalizer::predictKinematicsModel(double actual_dt) { Eigen::MatrixXd X_curr(dim_x_, 1); Eigen::MatrixXd X_next(dim_x_, 1); ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); Eigen::MatrixXd P_curr; ekf_.getLatestP(P_curr); const double yaw = X_curr(IDX::YAW); const double yaw_bias = X_curr(IDX::YAWB); const double vx = X_curr(IDX::VX); const double wz = X_curr(IDX::WZ); const double ax = X_curr(IDX::AX); const double wz_imu = X_curr(IDX::WZ_IMU); const double dt = actual_dt; /* Update for latest state */ X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt; X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt + 0.5 * ax * sin(yaw + yaw_bias) * dt * dt; X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz + wz_imu) * dt; X_next(IDX::YAWB) = yaw_bias; X_next(IDX::VX) = vx + ax * dt; X_next(IDX::WZ) = wz; X_next(IDX::AX) = ax; X_next(IDX::WZ_IMU) = wz_imu; X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW))); /* Set A matrix for latest state */ Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_); A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt - 0.5 * ax * sin(yaw + yaw_bias) * dt * dt; A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt - 0.5 * ax * sin(yaw + yaw_bias) * dt * dt; A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; A(IDX::X, IDX::AX) = 0.5 * cos(yaw + yaw_bias) * dt * dt; A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt; A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt; A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; A(IDX::Y, IDX::AX) = 0.5 * sin(yaw + yaw_bias) * dt * dt; A(IDX::YAW, IDX::WZ) = dt; A(IDX::YAW, IDX::WZ_IMU) = dt; A(IDX::VX, IDX::AX) = dt; /* Process noise covariance matrix Q */ Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_); // 位置过程噪声(由速度和加速度引起) const double dvx = std::sqrt(P_curr(IDX::VX, IDX::VX)); const double dax = std::sqrt(P_curr(IDX::AX, IDX::AX)); const double dyaw = std::sqrt(P_curr(IDX::YAW, IDX::YAW)); if (dvx < 10.0 && dyaw < 1.0 && dax < 5.0) { Eigen::MatrixXd Jp_pos = Eigen::MatrixXd::Zero(2, 3); Jp_pos << cos(yaw), -vx*sin(yaw), 0.5*cos(yaw), sin(yaw), vx*cos(yaw), 0.5*sin(yaw); Eigen::MatrixXd Q_vx_yaw_ax = Eigen::MatrixXd::Zero(3, 3); Q_vx_yaw_ax(0, 0) = P_curr(IDX::VX, IDX::VX) * dt; Q_vx_yaw_ax(1, 1) = P_curr(IDX::YAW, IDX::YAW) * dt; Q_vx_yaw_ax(2, 2) = P_curr(IDX::AX, IDX::AX) * dt; Q_vx_yaw_ax(0, 1) = P_curr(IDX::VX, IDX::YAW) * dt; Q_vx_yaw_ax(1, 0) = P_curr(IDX::YAW, IDX::VX) * dt; Q_vx_yaw_ax(0, 2) = P_curr(IDX::VX, IDX::AX) * dt; Q_vx_yaw_ax(2, 0) = P_curr(IDX::AX, IDX::VX) * dt; Q_vx_yaw_ax(1, 2) = P_curr(IDX::YAW, IDX::AX) * dt; Q_vx_yaw_ax(2, 1) = P_curr(IDX::AX, IDX::YAW) * dt; Q.block(0, 0, 2, 2) = Jp_pos * Q_vx_yaw_ax * Jp_pos.transpose(); } else { Q(IDX::X, IDX::X) = 0.1; Q(IDX::Y, IDX::Y) = 0.1; } // 角度过程噪声 Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_; Q(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // 速度过程噪声 Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // 加速度和IMU角速度过程噪声 Q(IDX::AX, IDX::AX) = proc_cov_ax_d_; Q(IDX::WZ_IMU, IDX::WZ_IMU) = proc_cov_wz_imu_d_; ekf_.predictWithDelay(X_next, A, Q); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } /* * measurementUpdatePose */ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::PoseStamped& pose) { if (pose.header.frame_id != pose_frame_id_) { ROS_WARN_DELAYED_THROTTLE(2, "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), pose_frame_id_.c_str()); } Eigen::MatrixXd X_curr(dim_x_, 1); // curent state ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output const ros::Time t_curr = ros::Time::now(); /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).toSec() + pose_additional_delay_; if (delay_time < 0.0) { delay_time = 0.0; ROS_WARN_DELAYED_THROTTLE(1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { ROS_WARN_DELAYED_THROTTLE(1.0, "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); return; } DEBUG_INFO("delay_time: %f [s]", delay_time); /* Set yaw */ const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW)); double yaw = tf2::getYaw(pose.pose.orientation); const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi yaw = yaw_error + ekf_yaw; /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << pose.pose.position.x, pose.pose.position.y, yaw; if (isnan(y.array()).any() || isinf(y.array()).any()) { ROS_WARN("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); return; } /* Gate */ Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; Eigen::MatrixXd P_curr, P_y; ekf_.getLatestP(P_curr); P_y = P_curr.block(0, 0, dim_y, dim_y); if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) { ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; } DEBUG_PRINT_MAT(y.transpose()); DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw /* Set measurement noise covariancs */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (use_pose_with_covariance_) { R(0, 0) = current_pose_covariance_.at(0); // x - x R(0, 1) = current_pose_covariance_.at(1); // x - y R(0, 2) = current_pose_covariance_.at(5); // x - yaw R(1, 0) = current_pose_covariance_.at(6); // y - x R(1, 1) = current_pose_covariance_.at(7); // y - y R(1, 2) = current_pose_covariance_.at(11); // y - yaw R(2, 0) = current_pose_covariance_.at(30); // yaw - x R(2, 1) = current_pose_covariance_.at(31); // yaw - y R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw } else { const double ekf_yaw = ekf_.getXelement(IDX::YAW); const double vx = ekf_.getXelement(IDX::VX); const double wz = ekf_.getXelement(IDX::WZ); const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0); const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0); const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0); R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x; // pos_x R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y; // pos_y R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw; // yaw } /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every * step. */ R *= (ekf_rate_ / pose_rate_); ekf_.updateWithDelay(y, C, R, delay_step); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } /* * measurementUpdateTwist */ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::TwistStamped& twist) { if (twist.header.frame_id != output_frame_id_) { ROS_WARN_DELAYED_THROTTLE(2.0, "twist frame_id must be %s", output_frame_id_.c_str()); } Eigen::MatrixXd X_curr(dim_x_, 1); // curent state ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 2; // vx, wz const ros::Time t_curr = ros::Time::now(); /* Calculate delay step */ double delay_time = (t_curr - twist.header.stamp).toSec() + twist_additional_delay_; if (delay_time < 0.0) { ROS_WARN_DELAYED_THROTTLE(1.0, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time); delay_time = 0.0; } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { ROS_WARN_DELAYED_THROTTLE(1.0, "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); return; } DEBUG_INFO("delay_time: %f [s]", delay_time); /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << twist.twist.linear.x, twist.twist.angular.z; if (isnan(y.array()).any() || isinf(y.array()).any()) { ROS_WARN("[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); return; } /* Gate */ Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); Eigen::MatrixXd P_curr, P_y; ekf_.getLatestP(P_curr); P_y = P_curr.block(4, 4, dim_y, dim_y); if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) { ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; } DEBUG_PRINT_MAT(y.transpose()); DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); C(0, IDX::VX) = 1.0; // for vx C(1, IDX::WZ) = 1.0; // for wz /* Set measurement noise covariancs */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (use_twist_with_covariance_) { R(0, 0) = current_twist_covariance_.at(0); // vx - vx R(0, 1) = current_twist_covariance_.at(5); // vx - wz R(1, 0) = current_twist_covariance_.at(30); // wz - vx R(1, 1) = current_twist_covariance_.at(35); // wz - wz } else { R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_ * ekf_dt_; // for vx R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_ * ekf_dt_; // for wz } /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */ R *= (ekf_rate_ / twist_rate_); ekf_.updateWithDelay(y, C, R, delay_step); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); }; /* * mahalanobisGate */ bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& x, const Eigen::MatrixXd& obj_x, const Eigen::MatrixXd& cov) { Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x); // DEBUG_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max); ROS_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max); if (mahalanobis_squared(0) > dist_max * dist_max) { return false; } return true; } /* * publishEstimateResult */ void EKFLocalizer::publishEstimateResult() { ros::Time current_time = ros::Time::now(); Eigen::MatrixXd X(dim_x_, 1); Eigen::MatrixXd P(dim_x_, dim_x_); ekf_.getLatestX(X); ekf_.getLatestP(P); /* publish latest pose */ pub_pose_.publish(current_ekf_pose_); /* publish latest pose with covariance */ geometry_msgs::PoseWithCovarianceStamped pose_cov; pose_cov.header.stamp = current_time; pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; pose_cov.pose.pose = current_ekf_pose_.pose; pose_cov.pose.covariance[0] = P(IDX::X, IDX::X); pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y); pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW); pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X); pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y); pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW); pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X); pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y); pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW); pub_pose_cov_.publish(pose_cov); /* publish latest twist */ pub_twist_.publish(current_ekf_twist_); /* publish latest twist with covariance */ geometry_msgs::TwistWithCovarianceStamped twist_cov; twist_cov.header.stamp = current_time; twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; twist_cov.twist.twist = current_ekf_twist_.twist; twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX); twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ) + P(IDX::VX, IDX::WZ_IMU); twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX) + P(IDX::WZ_IMU, IDX::VX); twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ) + P(IDX::WZ, IDX::WZ_IMU) + P(IDX::WZ_IMU, IDX::WZ) + P(IDX::WZ_IMU, IDX::WZ_IMU); pub_twist_cov_.publish(twist_cov); /* Send transform of pose */ broadcastTF(current_time); /* publish yaw bias */ std_msgs::Float64 yawb; yawb.data = X(IDX::YAWB); pub_yaw_bias_.publish(yawb); /* debug measured pose */ if (current_pose_ptr_ != nullptr) { geometry_msgs::PoseStamped p; p = *current_pose_ptr_; p.header.stamp = current_time; pub_measured_pose_.publish(p); } /* debug publish */ double RAD2DEG = 180.0 / 3.141592; double pose_yaw = 0.0; if (current_pose_ptr_ != nullptr) pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG; std_msgs::Float64MultiArray msg; msg.data.push_back(X(IDX::YAW) * RAD2DEG); // [0] ekf yaw angle msg.data.push_back(pose_yaw); // [1] measurement yaw angle msg.data.push_back(X(IDX::YAWB) * RAD2DEG); // [2] yaw bias msg.data.push_back(X(IDX::AX)); // [3] estimated x acceleration msg.data.push_back(X(IDX::WZ_IMU)); // [4] estimated wz from IMU pub_debug_.publish(msg); } double EKFLocalizer::normalizeYaw(const double& yaw) { return std::atan2(std::sin(yaw), std::cos(yaw)); } /* * measurementUpdateIMU */ void EKFLocalizer::measurementUpdateIMU(const sensor_msgs::Imu& imu) { Eigen::MatrixXd X_curr(dim_x_, 1); ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 2; // ax, wz const ros::Time t_curr = ros::Time::now(); /* Calculate delay step */ double delay_time = (t_curr - imu.header.stamp).toSec(); if (delay_time < 0.0) { ROS_WARN_DELAYED_THROTTLE(1.0, "IMU time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); delay_time = 0.0; } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { ROS_WARN_DELAYED_THROTTLE(1.0, "IMU delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); return; } DEBUG_INFO("delay_time: %f [s]", delay_time); /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << imu.linear_acceleration.x, imu.angular_velocity.z; if (isnan(y.array()).any() || isinf(y.array()).any()) { ROS_WARN("[EKF] IMU measurement matrix includes NaN or Inf. ignore update. check IMU message."); return; } /* Gate */ Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::AX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ_IMU); Eigen::MatrixXd P_curr, P_y; ekf_.getLatestP(P_curr); P_y = P_curr.block(IDX::AX, IDX::AX, dim_y, dim_y); if (!mahalanobisGate(imu_gate_dist_, y_ekf, y, P_y)) { ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] IMU measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; } DEBUG_PRINT_MAT(y.transpose()); DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); C(0, IDX::AX) = 1.0; // for ax C(1, IDX::WZ_IMU) = 1.0; // for wz_imu /* Set measurement noise covariance */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); R(0, 0) = imu_stddev_ax_ * imu_stddev_ax_; // for ax R(1, 1) = imu_stddev_wz_ * imu_stddev_wz_; // for wz_imu /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */ R *= (ekf_rate_ / imu_rate_); ekf_.updateWithDelay(y, C, R, delay_step); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); // Publish debug IMU message if (pub_measured_imu_.getNumSubscribers() > 0) { sensor_msgs::Imu debug_imu = imu; debug_imu.header.stamp = t_curr; pub_measured_imu_.publish(debug_imu); } } 小车转向时ekf发布的base_link在rviz中的转向不变但点云在转动,而且小车相对点云位置准确而且稳定

OpenSim::Logger::setLevel(OpenSim::Logger::Level::Error); OpenSim::Model model("C:/IKsolver/Model_Pose2Sim_Halpe26_scaled.osim"); SimTK::State& state = model.initSystem(); model.realizePosition(state); vector<string> labels{ "Hip", "RHip", "RKnee", "RAnkle", "RBigToe", "RSmallToe", "RHeel", "LHip", "LKnee", "LAnkle", "LBigToe", "LSmallToe", "LHeel", "Neck", "Head", "Nose", "RShoulder", "RElbow", "RWrist", "LShoulder", "LElbow", "LWrist"}; vector<double> order = {2, 2, 2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 1, 0.2, 0.0, 0.0, 2, 1, 1, 2, 1, 1}; Set<MarkerWeight> markerWeights; size_t nc = labels.size(); size_t nr = 1; for (size_t m{0}; m < nc; ++m) markerWeights.adoptAndAppend( new MarkerWeight(labels[m], double(order[m]))); SimTK::Array_<CoordinateReference> coordRefs; auto t0 = std::chrono::steady_clock::now(); TimeSeriesTable_<SimTK::Vec3> markerData; markerData.setColumnLabels(labels); SimTK::RowVector_<SimTK::Vec3> row(nc); for (size_t i = 0; i < 22; ++i) { row[i] = SimTK::Vec3(data[3*i], data[3*i+1], data[3*i+2]); } markerData.appendRow(0, row); MarkersReference markersRef(markerData, markerWeights); for (const auto& label : labels) { std::cout << label << ": " << markerWeights.get(label).getWeight() << std::endl; } for (int i = 0; i < model.getMarkerSet().getSize(); ++i) { std::cout << model.getMarkerSet()[i].getName() << std::endl; } OpenSim::InverseKinematicsSolver iksolver(model, markersRef, coordRefs, SimTK::Infinity); iksolver.setAccuracy(1.0e-9); try { iksolver.assemble(state); iksolver.track(state); } catch (const std::exception& e) { std::cerr << "IK solve failed: " << e.what() << std::endl; } model.realizePosition(state); CoordinateSet coordSet = model.getCoordinateSet(); try { model.realizePosition(state); auto val = coordSet[0].getValue(state); std::cout << "val: " << val << std::endl; } catch (const std::exception& e) { std::cerr << "Caught exception at getValue(): " << e.what() << std::endl; } auto t1 = std::chrono::steady_clock::now(); std::cout << "Elapsed: " << std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count() << " ms" << std::endl; SimTK::Array_<double> markerErrors; iksolver.computeCurrentMarkerErrors(markerErrors); double totalError = 0.0; for (size_t i = 0; i < markerErrors.size(); ++i) { double e = markerErrors[i]; totalError += e; std::cout << labels[i] << " error: " << e << " m" << std::endl; } std::cout << "Total marker error sum: " << totalError << " m" << std::endl; for (int i = 0; i < coordSet.getSize(); ++i){ std::cout << coordSet[i].getName() << ": " << coordSet[i].getValue(state) << std::endl; auto val = coordSet[i].getValue(state); if (std::isnan(val)) { std::cout << coordSet[i].getName() << ": NaN " << std::endl; } else { std::cout << coordSet[i].getName() << ": " << val << std::endl; } } cout << coordSet[0].getValue(state) << endl; IK沒辦法輸出為什麼 Hip: 2 RHip: 2 RKnee: 2 RAnkle: 1 RBigToe: 1 RSmallToe: 1 RHeel: 1 LHip: 2 LKnee: 1 LAnkle: 1 LBigToe: 1 LSmallToe: 1 LHeel: 1 Neck: 0.2 Head: 0 Nose: 0 RShoulder: 2 RElbow: 1 RWrist: 1 LShoulder: 2 LElbow: 1 LWrist: 1 Nose Head Neck LShoulder RShoulder LElbow RElbow LWrist RWrist Hip LHip RHip LKnee RKnee LAnkle RAnkle LBigToe LSmallToe LHeel RBigToe RSmallToe RHeel PS C:\IKsolver\build>

“#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::Ptr tree(new pcl::search::KdTree); tree->setInputCloud(cloud); // 2. 执行欧几里得聚类 - 确保连续性 std::vector cluster_indices; pcl::EuclideanClusterExtraction 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 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 ); } }”修改成你给我的样子,rviz观察到的四个tf坐标还是(0,0,0)

#include <iostream> #include "SerialPort.hpp" #include "Vision.hpp" #include <cstring> #include <string> #include "pid.hpp" #include <stdio.h> #include <cmath> extern SP::SerialPort serial; // 修改为你的串口设备 extern char msg[6]; //extern pid::PIDCLASS pid1; pid::PIDCLASS pid1( 1.40F, 0, 0.060F, 100.0F, 100.0F, -100.0F, 0.040F ); double th = 20;/* 二值化阈值 */ float Xposition = 0; float pidoutput = 0; int pidoutINT = 0; u_int8_t MX = 125; char STR[20]={0}; int main() { cv::VideoCapture Camera01;/* 定义相机 */ cv::Mat Picture0,Picture_B,Picture_Gray,inary,Picture_Binary_Deal,Picture_Binary_Deal_ROI, Picture_Binary_Deal_ROI_L,Picture_Binary_Deal_ROI_R,Picture_Binary;/* 定义图片 */ Camera01.open(0);/* 打开系统视频设备0 */ if( !Camera01.isOpened() ){std::cout<<"初始化时相机打开失败"<<std::endl;return -1;}/* 相机打开失败时 结束程序 */ Camera01.set(cv::CAP_PROP_FRAME_WIDTH,320);/* 设置相机分辨率 */ Camera01.set(cv::CAP_PROP_FRAME_HEIGHT,240); //uart_init(); uart_init(); while( Camera01.read(Picture0) ){ /* 读取图片到Picture0 */ int keyvalue = 0; usleep(40000);/* 40000us=40ms 休眠 */ cv::cvtColor(Picture0, Picture_Gray, cv::COLOR_BGR2GRAY);/* 灰度化 */ cv::threshold(Picture_Gray, Picture_Binary, th, 255, cv::THRESH_BINARY_INV); // 反二值化,黑线变白 cv::Mat Picture_element = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(5,5));//形态学操作 cv::morphologyEx(Picture_Binary, Picture_Binary_Deal, cv::MORPH_OPEN, Picture_element);//开运算 cv::Rect rect_roizone = cv::Rect(0,Picture_Binary_Deal.size().height/2, Picture_Binary_Deal.size().width,Picture_Binary_Deal.size().height/2); Picture_Binary_Deal_ROI = Picture_Binary_Deal( rect_roizone ); /* 设定两个ROI */ cv::Rect rect_roizone_Left = cv::Rect(0,0, Picture_Binary_Deal_ROI.size().width/2,Picture_Binary_Deal_ROI.size().height); cv::Rect rect_roizone_Right = cv::Rect( Picture_Binary_Deal_ROI.size().width/2 , 0 , Picture_Binary_Deal_ROI.size().width/2,Picture_Binary_Deal_ROI.size().height); Picture_Binary_Deal_ROI_L = Picture_Binary_Deal_ROI( rect_roizone_Left );/* 设定下半部分屏幕1/3 2/3区域为ROI */ Picture_Binary_Deal_ROI_R = Picture_Binary_Deal_ROI( rect_roizone_Right ); cv::Moments Moment1 = cv::moments(Picture_Binary_Deal_ROI,true);/* 算质心 */ cv::Moments Moment1L = cv::moments(Picture_Binary_Deal_ROI_L,true);/* 算左侧质心 */ cv::Moments Moment1R = cv::moments(Picture_Binary_Deal_ROI_R,true);/* 算右侧质心 */ float XmL = (float)(Moment1L.m10/Moment1L.m00), XmR = (float)(Moment1R.m10/Moment1R.m00); float temp= (float)(Moment1.m10/Moment1.m00); if( std::isnan(temp) ){ Xposition=0; } else{Xposition = temp;} //Xposition = /* 获得车道的X坐标 */ cv::Point p01( Xposition, Moment1.m01/Moment1.m00 + Picture_Binary.size().height/2);/* 求质心点 */ //cv::Point p01(Moment1.m10/Moment1.m00,Picture_Binary_Deal.size().height/2+Picture_Binary.size().height/2);/* 质心 */ pidoutput = pid1.pidf( (float)(Picture_Binary.size().width/2),(float)(Xposition) ); pidoutINT = (int)(pidoutput); std::cout << "pidout:"<0 50-> -70 由于\0是字符串结束符 故不能发送0 */ if(MX<1){ MX=1;}if(MX>255){MX=255;} sprintf(STR,"pidour:%d ",pidoutINT); cv::Point p03( XmL , Picture_Binary.size().height/2+Moment1L.m01/Moment1L.m00); cv::Point p04( Picture_Binary.size().width/2+XmR , Picture_Binary.size().height/2+Moment1R.m01/Moment1R.m00); cv::Point p00( Picture_Binary.size().width/2 , Picture_Binary.size().height ); cv::Point p02( Picture_Binary.size().width/2 + pidoutINT, Moment1.m01/Moment1.m00 + Picture_Binary.size().height/2);/* pid输出以点的形式呈现 */ cv::rectangle(Picture0,rect_roizone,cv::Scalar(165,0,0),2); cv::circle(Picture0, p02, 4, cv::Scalar(255, 0, 0), 2);/* 画点 */ cv::circle(Picture0, p01, 8, cv::Scalar(0, 255, 0), 2);/* 画点 */ cv::circle(Picture0, p03, 5, cv::Scalar(200, 233, 0), 1);/* 画左侧质心点 */ cv::circle(Picture0, p04, 5, cv::Scalar(200, 233, 0), 1);/* 画右侧质心点 */ std::cout << "XmL="<<XmL <<" XmR="<< (Picture_Binary.size().width/2+XmR) << std::endl; if( (!std::isnan(XmL)) && (!std::isnan(XmR) )&& (abs(XmL - XmR) > 100.0)) { //std::cout << "两条路" << std::endl; cv::line( Picture0, p00, p03, cv::Scalar(255, 0, 0), 1 ); } cv::line(Picture0, cv::Point( Picture0.size().width/2 , 0 ), cv::Point(Picture0.size().width/2, Picture0.size().height), cv::Scalar(0, 0, 200));/* 画中线 */ cv::putText(Picture0,STR,cv::Point(4,25),cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar_(200,200,100),2,8 ) ; //cv::imshow("WightBlackPic",Picture_Binary_Deal);/* 显示图片 */ cv::imshow("CapPicture",Picture0);/* 显示图片 */ cv::imshow("ROI",Picture_Binary_Deal_ROI);/* 显示图片 */ cv::imshow("ROI_Left",Picture_Binary_Deal_ROI_L); cv::imshow("ROI_Right",Picture_Binary_Deal_ROI_R); cv::moveWindow("ROI",450,0);cv::moveWindow("ROI_Left",450,160);cv::moveWindow("ROI_Right",450+320,160); cv::moveWindow("CapPicture",0,0); if( cv::waitKey(1) == 'q' ){std::cout<<"检测到按下q退出"<<std::endl;return 1;}/* 图片显示停留1ms 除非检测到按键'q' 退出程序 */ msg[0]='S'; msg[1]=MX; msg[2]=0xFF; msg[3]=0xFF; msg[4]='E'; //msg[5]=0; uart(); } return 0; }

geometry_msgs::msg::Twist calc_twist( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) { const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); const double dt_s = dt.seconds(); if (dt_s == 0) { //(20250310)Time Error! /* std::cout << "++++++++++++++++++++++++++++++++++++++++++++++"<<std::endl; std::cout << "Util_func(Twist Err) dt_s : "<<dt_s<<" "<<std::endl; std::cout << "+++++++++++++++++++++++++++++++++++++++++++++"<<std::endl; */ //printf("Util_func(Twist Err) dt_s ===%f \n", dt_s); return geometry_msgs::msg::Twist(); } const auto pose_a_rpy = get_rpy(pose_a); const auto pose_b_rpy = get_rpy(pose_b); geometry_msgs::msg::Vector3 diff_xyz; geometry_msgs::msg::Vector3 diff_rpy; diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); geometry_msgs::msg::Twist twist; twist.linear.x = diff_xyz.x / dt_s; twist.linear.y = diff_xyz.y / dt_s; twist.linear.z = diff_xyz.z / dt_s; twist.angular.x = diff_rpy.x / dt_s; twist.angular.y = diff_rpy.y / dt_s; twist.angular.z = diff_rpy.z / dt_s; //(20250310)Time Error! //if (dt_s < 0.001 || dt_s > 2.5 ) { /* std::cout << "==========================================================================="<<std::endl; std::cout << "Util_func(Twist) dt_s : "<<dt_s<<" "<<std::endl; std::cout << "Util_func(Twist) diff_xyz : "<<diff_xyz.x<<" "<<diff_xyz.y<<" "<<diff_xyz.z<<" "<<std::endl; std::cout << "==========================================================================="<<std::endl; */ // printf("Util_func(Twist) dt_s ===%f \n", dt_s); // printf("Util_func(Twist) diff_xyz ===%f %f %f\n", diff_xyz.x,diff_xyz.y,diff_xyz.z); //} return twist; }代码漏洞

出现问题迭代 1 | 总损失: 6.72196 | 距离损失: 10.1859 | 面积损失: 0.288881 0x00007FFE5A118508 (torch_cpu.dll)处(位于 test.exe 中)引发的异常: 0xC000008E: Floating-point division by zero (参数: 0x0000000000000000, 0x0000000000001924)。 “test.exe”(Win32): 已加载“C:\Windows\System32\duser.dll”。 “test.exe”(Win32): 已加载“C:\Windows\System32\xmllite.dll”。 “test.exe”(Win32): 已加载“C:\Windows\System32\atlthunk.dll”。 “test.exe”(Win32): 已卸载“C:\Windows\System32\xmllite.dll” 0x00007FFF5CE8933A (KernelBase.dll)处(位于 test.exe 中)引发的异常: 0x8001010D: 因为应用程序正在发送一个输入同步呼叫,所以无法执行传出的呼叫。。 “test.exe”(Win32): 已加载“C:\Windows\System32\OneCoreCommonProxyStub.dll”。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FDF50 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 线程 33260 已退出,返回值为 0 (0x0)。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE2B0 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE300 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FDF50 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE2B0 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE300 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 线程 36532 已退出,返回值为 0 (0x0)。 线程 1208 已退出,返回值为 0 (0x0)。 线程 36320 已退出,返回值为 0 (0x0)。 线程 22984 已退出,返回值为 0 (0x0)。 线程 4384 已退出,返回值为 0 (0x0)。 线程 17576 已退出,返回值为 0 (0x0)。 线程 14288 已退出,返回值为 0 (0x0)。 线程 35492 已退出,返回值为 0 (0x0)。 线程 22860 已退出,返回值为 0 (0x0)。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE330 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE3A0 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE330 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE3A0 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE330 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: wil::ResultException,位于内存位置 0x000000EDA68FE3A0 处。 0x00007FFF5CE8933A 处(位于 test.exe 中)引发的异常: Microsoft C++ 异常: [rethrow],位于内存位置 0x0000000000000000 处。 线程 30132 已退出,返回值为 0 (0x0)。 线程 27048 已退出,返回值为 0 (0x0)。 线程 27212 已退出,返回值为 0 (0x0)。 线程 33764 已退出,返回值为 0 (0x0)。 线程 35108 已退出,返回值为 0 (0x0)。 线程 12392 已退出,返回值为 0 (0x0)。 #define PARTICLESWARMMETHOD_EXPORTS // 定义导出宏 #include "particleSwarmMethod.h" torch::Tensor distance_point_to_segment(const torch::Tensor& P, const torch::Tensor& A, const torch::Tensor& B) { auto AP = P - A; auto AB = B - A; auto ab_sq = AB.pow(2).sum(); if (ab_sq.item<float>() == 0) { return AP.norm(2, 1); } auto t = torch::mv(AP, AB.squeeze()) / ab_sq; t = torch::clamp(t, 0.0, 1.0); auto proj = A + t.unsqueeze(1) * AB; return (P - proj).norm(2, 1); } std::vector<std::vector<float>> particle(const std::vector<float>& wy, const std::vector<float>& zh) { std::vector<std::vector<float>> particles;//六边形初始化 auto wymax = *std::max_element(wy.begin(), wy.end()); auto wymin = *std::min_element(wy.begin(), wy.end()); auto zhmax = *std::max_element(zh.begin(), zh.end()); auto zhmin = *std::min_element(zh.begin(), zh.end()); particles.push_back({ wymin, zhmax }); particles.push_back({ wymax, zhmax }); particles.push_back({ wymax, (zhmax - zhmin) * 0.5f + zhmin }); particles.push_back({ (wymax - wymin) * 0.5f + wymin, (zhmax - zhmin) * 0.5f + zhmin }); particles.push_back({ (wymax - wymin) * 0.5f + wymin, zhmin }); particles.push_back({ wymin, zhmin }); return particles; } // 应用约束 torch::Tensor apply_constraints(torch::Tensor vertices) { torch::NoGradGuard no_grad; // 临时禁用梯度计算 // 边界约束 vertices.data() = torch::clamp(vertices, -0.05, 1.02); auto min_val = torch::tensor(0.2, vertices.options()); auto max_val = vertices.index({ 1, 0 }) * 1.1; vertices.index_put_({ 2, 0 }, torch::clamp(vertices.index({ 2, 0 }), min_val, max_val)); vertices.index_put_({ 2, 1 }, torch::clamp(vertices.index({ 2, 1 }), 0.2, 1.02)); vertices.index_put_({ 3, 0 }, torch::clamp(vertices.index({ 3, 0 }), vertices.index({ 4, 0 }), vertices.index({ 2, 0 }) * 1.1)); vertices.index_put_({ 3, 1 }, torch::clamp(vertices.index({ 3, 1 }), vertices.index({ 4, 1 }) * 1.1, vertices.index({ 2, 1 }))); vertices.index_put_({ 4, 0 }, torch::clamp(vertices.index({ 4, 0 }), vertices.index({ 5, 0 }) * 1.1, vertices.index({ 3, 0 }))); vertices.index_put_({ 4, 1 }, torch::clamp(vertices.index({ 4, 1 }), torch::nullopt, vertices.index({ 5, 1 }) + 0.1)); return vertices; } // 计算损失函数 std::tuple<torch::Tensor, torch::Tensor, torch::Tensor> calculate_gradient_descent_loss(const torch::Tensor& vertices, const torch::Tensor& points, const torch::Tensor& target_area, float lambda1) { // 计算六边形面积 auto shifted_vertices = torch::roll(vertices, -1, 0); auto x = vertices.index({ torch::indexing::Slice(), 0 }); auto y = vertices.index({ torch::indexing::Slice(), 1 }); auto x_shifted = shifted_vertices.index({ torch::indexing::Slice(), 0 }); auto y_shifted = shifted_vertices.index({ torch::indexing::Slice(), 1 }); auto area = 0.5 * torch::abs(torch::sum(x * y_shifted - y * x_shifted)); // 计算所有边到数据点的距离 std::vector<std::pair<int, int>> edges; for (int i = 0; i < 6; ++i) { edges.emplace_back(i, (i + 1) % 6); // 添加边 (i, (i+1)%6) } std::vector<torch::Tensor> dists; for (const auto& edge : edges) { auto A = vertices[edge.first].unsqueeze(0); // [1, 2] auto B = vertices[edge.second].unsqueeze(0); // [1, 2] auto dist = distance_point_to_segment(points, A, B); dists.push_back(dist); } auto dist_matrix = torch::stack(dists, 1); auto min_dists = std::get<0>(torch::min(dist_matrix, 1)); auto distance_loss = torch::sum(min_dists); // 计算面积损失 auto area_loss = torch::abs(area - target_area); // 总损失 auto total_loss = (1 - lambda1) * area_loss + lambda1 * distance_loss; return { total_loss, distance_loss, area_loss }; } extern "C" PARTICLESWARMMETHOD_API int* opening_closing_point_recognition( const float* wy, int wySize, const float* zh, int zhSize, int iterations, bool print1, float lambda1, int* resultSize ) { // 输出所有输入信息 std::cout << "Opening_closing_point_recognition 函数的输入信息:" << std::endl; std::cout << "wy 数组内容:"; for (int i = 0; i < wySize; ++i) { std::cout << wy[i]; if (i < wySize - 1) { std::cout << ", "; } } std::cout << std::endl; std::cout << "wy 数组大小: " << wySize << std::endl; std::cout << "zh 数组内容:"; for (int i = 0; i < zhSize; ++i) { std::cout << zh[i]; if (i < zhSize - 1) { std::cout << ", "; } } std::cout << std::endl; std::cout << "zh 数组大小: " << zhSize << std::endl; std::cout << "迭代次数: " << iterations << std::endl; std::cout << "是否打印信息: " << (print1 ? "是" : "否") << std::endl; std::cout << "权重系数 lambda1: " << lambda1 << std::endl; // 数据归一化 auto wmin = *std::min_element(wy, wy + wySize); auto wmax = *std::max_element(wy, wy + wySize); auto zmin = *std::min_element(zh, zh + zhSize); auto zmax = *std::max_element(zh, zh + zhSize); std::vector<float> wy_nored(wySize), zh_nored(zhSize); for (int i = 0; i < wySize; ++i) { wy_nored[i] = (wy[i] - wmin) / (wmax - wmin); } for (int i = 0; i < zhSize; ++i) { zh_nored[i] = (zh[i] - zmin) / (zmax - zmin); } std::vector<float> low_before = { *std::min_element(wy_nored.begin(), wy_nored.end()) - 0.1f, *std::min_element(zh_nored.begin(), zh_nored.end()) - 0.1f }; std::vector<float> up_before = { *std::max_element(wy_nored.begin(), wy_nored.end()) * 1.1f, *std::max_element(zh_nored.begin(), zh_nored.end()) * 1.1f }; std::vector<float> low, up; for (int i = 0; i < 6; ++i) { low.insert(low.end(), low_before.begin(), low_before.end()); up.insert(up.end(), up_before.begin(), up_before.end()); } // 初始化六边形 std::vector<std::vector<float>> polygon = particle(wy_nored, zh_nored); // 将 polygon (vector<vector<float>>) 转换为 Tensor std::vector<float> flattened; for (const auto& point : polygon) { flattened.insert(flattened.end(), point.begin(), point.end()); } torch::Tensor vertices = torch::tensor(flattened, torch::dtype(torch::kFloat32)) .view({ -1, 2 }) .requires_grad_(true); // 创建优化器 auto optimizer = torch::optim::AdamW({ vertices }, torch::optim::AdamWOptions(0.008)); //auto scheduler = torch::optim::LRScheduler(optimizer, 500); // 准备数据点 std::vector<torch::Tensor> point_tensors; for (size_t i = 0; i < wy_nored.size(); ++i) { point_tensors.push_back(torch::tensor({ wy_nored[i], zh_nored[i] })); } auto points = torch::stack(point_tensors); std::cout << "vertices 张量的值:" << std::endl; auto vertices_accessor = points.accessor<float, 2>(); for (int i = 0; i < points.size(0); ++i) { for (int j = 0; j < points.size(1); ++j) { std::cout << vertices_accessor[i][j] << " "; } std::cout << std::endl; } // 计算目标面积 auto shifted_points = torch::roll(points, -1, 0); auto target_area = 0.5 * torch::abs(torch::sum( points.index({ torch::indexing::Slice(), 0 }) * shifted_points.index({ torch::indexing::Slice(), 1 }) - points.index({ torch::indexing::Slice(), 1 }) * shifted_points.index({ torch::indexing::Slice(), 0 }))); // 优化循环 for (int k = 0; k < iterations; ++k) { optimizer.zero_grad(); auto loss = calculate_gradient_descent_loss(vertices, points, target_area, lambda1); //[total_loss, distance_loss, area_loss] torch::Tensor total_loss = std::get<0>(loss); // 第一个元素 torch::Tensor distance_loss = std::get<1>(loss); // 第二个元素 torch::Tensor area_loss = std::get<2>(loss); // 第三个元素 // 输出损失信息 std::cout << "迭代 " << k + 1 << " | 总损失: " << total_loss.item<float>() << " | 距离损失: " << distance_loss.item<float>() << " | 面积损失: " << area_loss.item<float>() << std::endl; try { total_loss.backward(); } catch (const c10::Error& e) { std::cerr << "Caught c10::Error: " << e.what() << std::endl; } catch (const std::exception& e) { std::cerr << "Caught std::exception: " << e.what() << std::endl; } vertices = apply_constraints(vertices); optimizer.step(); /* // 打印信息 std::cout << "迭代次数 " << k + 1 << ", 面积损失: " << area_loss.item<float>() << ", 距离损失: " << distance_loss.item<float>() << ", 总损失: " << total_loss.item<float>() << std::endl; */ } // 找到最近点 std::vector<int> vertex_indices = { 0, 1, 4, 5 }; std::vector<std::vector<float>> closest_points; for (int idx : vertex_indices) { auto vertex = vertices.index({ idx }).unsqueeze(0); auto distances = torch::norm(points - vertex, 2, 1); auto closest_idx = torch::argmin(distances).item<int>(); auto closest_point = points.index({ closest_idx }); closest_points.push_back({ closest_point.index({0}).item<float>(), closest_point.index({1}).item<float>() }); } // 反归一化 std::vector<std::vector<float>> closest_points_original; for (const auto& point : closest_points) { closest_points_original.push_back({ point[0] * (wmax - wmin) + wmin, point[1] * (zmax - zmin) + zmin }); } // 匹配原始数据点索引 std::vector<int> matched_indices; for (const auto& point : closest_points_original) { for (int i = 0; i < wySize; i++) { if (round(point[0] * 100.0f) / 100.0f == round(wy[i] * 100.0f) / 100.0f && round(point[1] * 100.0f) / 100.0f == round(zh[i] * 100.0f) / 100.0f) { matched_indices.push_back(i); break; } } } // 打印结果 if (print1) { std::cout << "反归一化后的最近点坐标:" << std::endl; for (size_t i = 0; i < closest_points_original.size(); ++i) { std::cout << "顶点 " << vertex_indices[i] << " 最近点: [" << closest_points_original[i][0] << ", " << closest_points_original[i][1] << "]" << std::endl; } } *resultSize = static_cast<int>(matched_indices.size()); // 动态分配结果数组 int* output = new int[matched_indices.size()]; std::copy(matched_indices.begin(), matched_indices.end(), output); return output; } extern "C" PARTICLESWARMMETHOD_API void free_int_array(int* arr) { delete[] arr; }

#ifndef __INCLUDE_COMTRADE_FORMAT_H 118: #define __INCLUDE_COMTRADE_FORMAT_H 119: 120: 121: 122: /* 123: * 头文件 124: */ 125: #include "base_type.h" 126: 127: 128: #if defined (__cplusplus) 129: extern "C" { 130: #endif /* defined (__cplusplus) */ 131: 132: 133: /* 134: * 宏开关 定义为文件读写 135: */ 136: #define CMTR_IOFILE 137: 138: 139: 140: /* 141: * 宏定义文件的读写操作,可以根据需要改写该接口,如重定义 142: * 为网口的recv\send、串口r\w等。 143: * 144: * _my_read_cmtr_bufn/_my_read_cmtr_bufn - cmtr的读写操作 145: * @pfd: 读写地址,可以为文件的fd、或者buffer地址等 146: * @buf: 缓冲区地址 147: * @count: 需要读写的字节数 148: * 149: */ 150: #if defined(CMTR_IOFILE) 151: typedef int _my_cmtr_ioptr; 152: #define _my_read_cmtr_bufn(pfd, buf, count) \ 153: do { \ 154: if (read((pfd), (buf), (count)) <= 0) { \ 155: (pfd) = -1; \ 156: } \ 157: } while(0); 158: #define _my_write_cmtr_bufn(pfd, buf, count) \ 159: do { \ 160: if (write((pfd), (buf), (count)) <= 0) {\ 161: (pfd) = -1; \ 162: } \ 163: } while(0); 164: #define _my_check_cmtr_ptr(pfd) \ 165: (((pfd) != -1) && ((pfd) != 0)) 166: #elif defined(CMTR_IOBUFFER) 167: typedef u8* _my_cmtr_ioptr; 168: #define _my_read_cmtr_bufn(pfd, buf, count) \ 169: do { \ 170: memcpy((buf), (pfd), (count)); \ 171: (pfd) += (count); \ 172: } while(0); 173: #define _my_write_cmtr_bufn(pfd, buf, count) \ 174: do { \ 175: memcpy((pfd), (buf), (count)); \ 176: (pfd) += (count); \ 177: } while(0); 178: #define _my_check_cmtr_ptr(pfd) \ 179: (((pfd) != -1) && ((pfd) != 0)) 180: #endif 181: 182: 183: 184: /* 185: * 关于comtrade文件配置文件的宏定义。 186: * 在此采用预定义最大个数方法,避免动态内存管理的问题。 187: * 188: * 相关字符串最大字符个数,一般不会超过16个; 189: * 模拟量端口最大个数,一般不会超过64个; 190: * 数字量量端口最大个数,一般不会超过64个; 191: * 采样率最大个数,一般不会超过8个; 192: * 193: */ 194: #define CMTR_STRING_MAX_LENGTH 64 195: #define CMTR_ANALOG_MAX_COUNT 255 196: #define CMTR_DIGIT_MAX_COUNT 255 197: #define CMTR_SMPRATE_MAX_COUNT 255 198: 199: 200: 201: /* 202: * cmtr_cfg_analog - 配置文件模拟量信息 203: * @index: 模拟量端口序号(只是编号,解析时不做序号用); 204: * @name: 模拟量端口名称; 205: * @phase: 模拟量端口相标识,值如(A、B、C、N等); 206: * @element:标识(还未知,待补充),一般为空; 207: * @unit: 模拟量端口数值的单位,该单位常用来区分该端口是电流还是电流,值如:kV、V、A、mA等; 208: * @factor_a: 系数a,一般为整数,可以是浮点数; 209: * @factor_b: 系数b,一般为整数,可以是浮点数; 210: * @offset_time: 时间偏移,指第一个点的时间偏移量,一般为0; 211: * @smp_min: 通道的最小采用值,一般为整数,国内有些变态的仪器会生成浮点数(在次不支持); 212: * @smp_max: 通道的最大采用值,一般为整数,国内有些变态的仪器会生成浮点数(在次不支持); 213: * 214: * 通道采样的实际值计算方法:实际值 = factor_a * smp_value + factor_b;所以根据该公式,可以计算 215: * 通道的最小值为:factor_a * smp_min + factor_b,最大值为:factor_a * smp_max + factor_b。 216: * 217: * 注:本来smp_min、smp_max为两个字节的一个数据(即最大为65535),但不同厂家会生成很大的四字节数据, 218: * 所以采用s32类型;factor_a、factor_b用double类型,用float可能会丢精度;为了提供解析程序的适 219: * 应性,以适应国内各种变态的有标准不遵循的厂家的仪器生成的cmtr文件。 220: * 221: */ 222: struct cmtr_cfg_analog { 223: s32 index; 224: u8 name[CMTR_STRING_MAX_LENGTH]; 225: u8 phase[CMTR_STRING_MAX_LENGTH]; 226: u8 element[CMTR_STRING_MAX_LENGTH]; 227: u8 unit[CMTR_STRING_MAX_LENGTH]; 228: f64 factor_a; 229: f64 factor_b; 230: s32 offset_time; 231: s32 smp_min; 232: s32 smp_max; 233: }; 234: 235: 236: /* 237: * cmtr_cfg_digit - 配置文件数字量信息 238: * @index: 数字量端口序号(只是编号,解析时不做序号用); 239: * @name: 数字量端口名称; 240: * @state: 数字量起始状态值,一般为1或者0,很少情况下会为2; 241: * 242: */ 243: struct cmtr_cfg_digit { 244: s32 index; 245: u8 name[CMTR_STRING_MAX_LENGTH]; 246: s8 state; 247: }; 248: 249: /* 250: * cmtr_cfg_smprate_info - 配置文件采样点信息 251: * @rate: 采样率,一般为整数,也有小数表示的; 252: * @point: 该采样率下采样的点数,为整数; 253: * 254: */ 255: struct cmtr_cfg_smprate { 256: f32 rate; 257: s32 point; 258: }; 259: 260: /* 261: * cmtr_cfg_info - 配置文件信息。 262: * @station_name: 厂站名称; 263: * @kymograph_id: 录波器编号; 264: * @analog_count: 模拟量个数; 265: * @digit_count: 数字量个数; 266: * @analogs: 模拟量信息; 267: * @digits: 数字量信息; 268: * @frequency: 基本频率,一般为额定频率,指的是电网频率; 269: * @smprate_count: 采样率个数; 270: * @smprates: 采样率信息; 271: * @begin_time: 录波开始时间; 272: * @end_time: 录波结束时间; 273: * @file_type: 数据文件类型,可以“ASCII”和“Binary”,ASCII类型为dat文件可以用记事本打开看详 274: * 细的采样信息;binary格式的只能用特殊的工具查看,为二进制数据文件; 275: * 276: */ 277: typedef struct cmtr_cfg_info { 278: u8 station_name[CMTR_STRING_MAX_LENGTH]; 279: u8 kymograph_id[CMTR_STRING_MAX_LENGTH]; 280: s32 analog_count; 281: s32 digit_count; 282: struct cmtr_cfg_analog analogs[CMTR_ANALOG_MAX_COUNT]; 283: struct cmtr_cfg_digit digits[CMTR_DIGIT_MAX_COUNT]; 284: f32 frequency; 285: s32 smprate_count; 286: struct cmtr_cfg_smprate smprates[CMTR_SMPRATE_MAX_COUNT]; 287: u8 begin_time[CMTR_STRING_MAX_LENGTH]; 288: u8 end_time[CMTR_STRING_MAX_LENGTH]; 289: u8 file_type[CMTR_STRING_MAX_LENGTH]; 290: }; 291: 292: 293: 294: /* 295: * cmtr_dat_smpdot - 数据文件中的采样点数据信息. 296: * @index: 端口序号(只是编号,解析时不做序号用); 297: * @time: 采样点采样时间偏移量,单位微妙; 298: * @analogs: 模拟量信息,一般为有符号整数,国内有些变态的仪器会生成浮点数(在此不支持); 299: * @digits: 数字量信息,为有符号整数; 300: * 301: */ 302: struct cmtr_dat_smpdot{ 303: s32 index; 304: s32 time; 305: s32 analogs[CMTR_ANALOG_MAX_COUNT]; 306: s8 digits[CMTR_DIGIT_MAX_COUNT]; 307: }; 308: 309: 310: 311: 312: 313: /* 314: * write_cmtr_cfg_info - 写cmtr配置文件. 315: * @pfd: 输入输出参数,地址 316: * @cfg:输入参数,cmtr(cfg文件)结构体 317: * @counter: 输出参数,写入的字节计数器; 318: * 319: * 返回当前pfd指针,写失败返回NULL 320: * 321: */ 322: _my_cmtr_ioptr write_cmtr_cfg_info(_my_cmtr_ioptr pfd, 323: struct cmtr_cfg_info *cfg, 324: int *counter); 325: 326: 327: /* 328: * write_cmtr_dat_smpdot_ascii - 写cmtr采样点数据信息(ascii格式). 329: * @pfd: 输入输出参数,地址 330: * @analog_count: 输入参数,模拟量个数; 331: * @digit_count: 输入参数,数字量个数; 332: * @dot: 输入参数,采样点信息; 333: * @counter: 输出参数,写入的字节计数器; 334: * 335: * 返回当前pfd指针,写失败返回NULL 336: * 337: */ 338: _my_cmtr_ioptr write_cmtr_dat_smpdot_ascii(_my_cmtr_ioptr pfd, 339: int analog_count, int digit_count, 340: struct cmtr_dat_smpdot *dot, 341: int* counter); 342: 343: /* 344: * write_cmtr_dat_smpdot_binary - 写cmtr采样点数据信息(binary格式). 345: * @pfd: 输入输出参数,地址 346: * @big_endian_tag: 输入参数,标识文件中数据的字节顺序,为FALSE按照小端写(默认值),True按照大端写; 347: * @analog_count: 输入参数,模拟量个数; 348: * @digit_count: 输入参数,数字量个数; 349: * @dot: 输入参数,采样点信息; 350: * @counter: 输出参数,写入的字节计数器; 351: * 352: * 返回当前pfd指针,写失败返回NULL 353: * 354: */ 355: _my_cmtr_ioptr write_cmtr_dat_smpdot_binary(_my_cmtr_ioptr pfd, 356: u8 big_endian_tag, 357: int analog_count, int digit_count, 358: struct cmtr_dat_smpdot *dot, 359: int* counter); 360: 361: 362: /* 363: * read_cmtr_cfg_info - 读cmtr配置文件. 364: * @pfd: 输入输出参数,地址 365: * @cfg:输出参数,cmtr(cfg文件)结构体 366: * @counter: 输出参数,读取的字节计数器; 367: * 368: * 返回当前pfd指针,读失败返回NULL 369: * 370: */ 371: _my_cmtr_ioptr read_cmtr_cfg_info(_my_cmtr_ioptr pfd, 372: struct cmtr_cfg_info *cfg, 373: int* counter); 374: 375: 376: /* 377: * read_cmtr_dat_smpdot_ascii - 读ascii格式cmtr采样点数据信息. 378: * @pfd: 输入输出参数,地址 379: * @read_buf: 输入参数,读缓冲,在外部申请内存,避免在内部频繁申请、释放内存; 380: * @buf_size: 输入参数,读缓冲区的大小; 381: * @analog_count: 输入参数,模拟量个数; 382: * @digit_count: 输入参数,数字量个数; 383: * @dot: 输出参数,采样点信息; 384: * @the_smp_mins: 输出参数,统计最小值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 385: * @the_smp_maxs: 输出参数,统计最大值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 386: * @counter: 输出参数,读取的字节计数器; 387: * 388: * 返回当前pfd指针,读失败返回NULL 389: * 390: */ 391: _my_cmtr_ioptr read_cmtr_dat_smpdot_ascii(_my_cmtr_ioptr pfd, 392: u8 *read_buf, int buf_size, 393: int analog_count, int digit_count, 394: struct cmtr_dat_smpdot *dot, 395: u16* the_smp_mins, u16* the_smp_maxs, 396: int* counter); 397: 398: /* 399: * read_cmtr_dat_smpdot_binary - 读bin格式cmtr采样点数据信息. 400: * @pfd: 输入输出参数,地址 401: * @big_endian_tag: 输入参数,标识文件中数据的字节顺序,为FALSE按照小端读(默认值),True按照大端读; 402: * @analog_count: 输入参数,模拟量个数; 403: * @digit_count: 输入参数,数字量个数; 404: * @dot: 输出参数,采样点信息; 405: * @the_smp_mins: 输出参数,统计最小值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 406: * @the_smp_maxs: 输出参数,统计最大值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 407: * @counter: 输出参数,读取的字节计数器; 408: * 409: * 返回当前pfd指针,读失败返回NULL 410: * 411: */ 412: _my_cmtr_ioptr read_cmtr_dat_smpdot_binary(_my_cmtr_ioptr pfd, u8 big_endian_tag, 413: int analog_count, int digit_count, 414: struct cmtr_dat_smpdot *dot, 415: u32* the_smp_mins, u32* the_smp_maxs, 416: int* counter); 417: 418: /* 419: * print_cmtr_cfg_info - 打印cmtr文件配置数. 420: * @cfg: 输入参数,cmtr文件配置数据; 421: * 422: */ 423: void print_cmtr_cfg_info(struct cmtr_cfg_info *cfg); 424: 425: /* 426: * print_cmtr_dat_smpdot - 打印cmtr数据文件采样点. 427: * @cfg: 输入参数,cmtr文件配置数据; 428: * @analog_count: 输入参数,模拟量个数; 429: * @digit_count: 输入参数,数字量个数; 430: * @dot: 输入参数,采样点信息; 431: * 432: */ 433: void print_cmtr_dat_smpdot(int analog_count, int digit_count, 434: struct cmtr_dat_smpdot *dot); 435: 436: 437: 438: #if defined (__cplusplus) 439: } 440: #endif /* defined (__cplusplus) */ 441: 442: 443: #endif /* __INCLUDE_COMTRADE_FORMAT_H */ comtrade_format.c 1: /* 2: * 分析解析COMTRADE(IEEE标准电力系统暂态数据交换通用格式)文件格式。 3: * 4: * 5: * 本代码支持win32平台和linux平台。 6: * 7: * Copyright,lizhi<ibox> 8: * 9: * 2012-10-10 V1.0 lizhi<QQ:252240557,msn:[email protected]> created 10: * 11: */ 12: 13: /* 14: * 头文件 15: */ 16: #include "base_type.h" 17: #include "base_include.h" 18: #include "base_debug.h" 19: #include "base_endian.h" 20: #include "base_function.h" 21: #include "comtrade_format.h" 22: 23: 24: /* 25: * 测试宏开关 26: */ 27: ///* 28: #define CMTR_CONSOLE_DEMO 29: //*/ 30: 31: 32: /* 33: * _my_read_cmtr_data/_my_write_cmtr_data - 读取/写入并且转换cmtr数据。 34: * @src: 读写地址,为buffer地址; 35: * @data: 读取/写入的数据; 36: * @count: 需要读取/写入的字节个数; 37: * @counter: 读取/写入的字节计数器; 38: * 39: */ 40: #define _my_read_cmtr_data(src, data, count, counter) \ 41: do { \ 42: if (_my_check_cmtr_ptr(src)) { \ 43: _my_read_cmtr_bufn((src), (data), (count)); \ 44: (counter) += (count); \ 45: } \ 46: } while(0); 47: #define _my_write_cmtr_data(src, data, count, counter) \ 48: do { \ 49: if (_my_check_cmtr_ptr(src)) { \ 50: _my_write_cmtr_bufn((src), (data), (count)); \ 51: (counter) +=(count); \ 52: } \ 53: } while(0); 54: 55: /* 56: * write_cmtr_cfg_info - 写cmtr配置文件. 57: * @pfd: 输入输出参数,地址 58: * @cfg:输入参数,cmtr(cfg文件)结构体 59: * @counter: 输出参数,写入的字节计数器; 60: * 61: * 返回当前pfd指针,写失败返回NULL 62: * 63: */ 64: _my_cmtr_ioptr write_cmtr_cfg_info(_my_cmtr_ioptr pfd, struct cmtr_cfg_info *cfg, int *counter) 65: { 66: u32 strlenr = 0; 67: u32 strmaxl = CMTR_STRING_MAX_LENGTH * 10; 68: u8* strline = (u8 *)_my_buf_malloc((size_t)(strmaxl)); 69: u32 index = 0; 70: 71: _my_cmtr_ioptr curr_pfd = pfd; 72: 73: memset(strline, '\0', strmaxl); 74: strlenr = sprintf(strline, "%s,%s\n", cfg->station_name, cfg->kymograph_id); 75: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 76: 77: memset(strline, '\0', strmaxl); 78: strlenr = sprintf(strline, "%d,%dA,%dD\n", cfg->analog_count + cfg->digit_count, cfg->analog_count, cfg->digit_count); 79: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 80: 81: for (index = 0; index < cfg->analog_count; index++) { 82: memset(strline, '\0', strmaxl); 83: strlenr = sprintf(strline, "%d,%s,%s,%s,%s,%lf,%lf,%d,%d,%d\n", 84: cfg->analogs[index].index, 85: cfg->analogs[index].name, 86: cfg->analogs[index].phase, 87: cfg->analogs[index].element, 88: cfg->analogs[index].unit, 89: cfg->analogs[index].factor_a, 90: cfg->analogs[index].factor_b, 91: cfg->analogs[index].offset_time, 92: cfg->analogs[index].smp_min, 93: cfg->analogs[index].smp_max); 94: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 95: } 96: 97: for (index = 0; index < cfg->digit_count; index++) { 98: memset(strline, '\0', strmaxl); 99: strlenr = sprintf(strline, "%d,%s,%d\n", 100: cfg->digits[index].index, 101: cfg->digits[index].name, 102: cfg->digits[index].state); 103: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 104: } 105: 106: memset(strline, '\0', strmaxl); 107: strlenr = sprintf(strline, "%f\n", cfg->frequency); 108: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 109: 110: memset(strline, '\0', strmaxl); 111: strlenr = sprintf(strline, "%d\n", cfg->smprate_count); 112: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 113: for (index = 0; index < cfg->smprate_count; index++) { 114: memset(strline, '\0', strmaxl); 115: strlenr = sprintf(strline, "%f,%d\n", cfg->smprates[index].rate, cfg->smprates[index].point); 116: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 117: } 118: 119: memset(strline, '\0', strmaxl); 120: strlenr = sprintf(strline, "%s\n", cfg->begin_time); 121: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 122: memset(strline, '\0', strmaxl); 123: strlenr = sprintf(strline, "%s\n", cfg->end_time); 124: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 125: strlenr = sprintf(strline, "%s\n", cfg->file_type); 126: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 127: 128: _my_buf_free(strline); 129: 130: return curr_pfd; 131: } 132: 133: 134: /* 135: * write_cmtr_dat_smpdot_ascii - 写cmtr采样点数据信息(ascii格式). 136: * @pfd: 输入输出参数,地址 137: * @analog_count: 输入参数,模拟量个数; 138: * @digit_count: 输入参数,数字量个数; 139: * @dot: 输入参数,采样点信息; 140: * @counter: 输出参数,写入的字节计数器; 141: * 142: * 返回当前pfd指针,写失败返回NULL 143: * 144: */ 145: _my_cmtr_ioptr write_cmtr_dat_smpdot_ascii(_my_cmtr_ioptr pfd, int analog_count, int digit_count, struct cmtr_dat_smpdot *dot, int* counter) 146: { 147: u32 strlenr = 0; 148: u32 strmaxl = CMTR_STRING_MAX_LENGTH; 149: u8* strline = (u8 *)_my_buf_malloc((size_t)(strmaxl)); 150: int index = 0; 151: 152: _my_cmtr_ioptr curr_pfd = pfd; 153: 154: _my_assert(pfd && dot); 155: 156: memset(strline, '\0', strmaxl); 157: strlenr = sprintf(strline, "%d,%d", dot->index, dot->time); 158: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 159: for (index = 0; index < analog_count; index++) { 160: memset(strline, '\0', strmaxl); 161: strlenr = sprintf(strline, ",%d", dot->analogs[index]); 162: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 163: } 164: for (index = 0; index < digit_count; index++) { 165: memset(strline, '\0', strmaxl); 166: strlenr = sprintf(strline, ",%d", dot->digits[index]); 167: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 168: } 169: memset(strline, '\0', strmaxl); 170: strlenr = sprintf(strline, "\n"); 171: _my_write_cmtr_data(curr_pfd, strline, strlenr, *counter); 172: 173: _my_buf_free(strline); 174: 175: return curr_pfd; 176: } 177: 178: 179: /* 180: * write_cmtr_dat_smpdot_binary - 写cmtr采样点数据信息(binary格式). 181: * @pfd: 输入输出参数,地址 182: * @big_endian_tag: 输入参数,标识文件中数据的字节顺序,为FALSE按照小端写(默认值),True按照大端写; 183: * @analog_count: 输入参数,模拟量个数; 184: * @digit_count: 输入参数,数字量个数; 185: * @dot: 输入参数,采样点信息; 186: * @counter: 输出参数,写入的字节计数器; 187: * 188: * 返回当前pfd指针,写失败返回NULL 189: * 190: */ 191: _my_cmtr_ioptr write_cmtr_dat_smpdot_binary(_my_cmtr_ioptr pfd, u8 big_endian_tag, int analog_count, int digit_count, struct cmtr_dat_smpdot *dot, int* counter) 192: { 193: s16 data16 = 0x00; 194: s32 data32 = 0x00; 195: int oldnum = 0; 196: 197: s16 datatp = 0x00; 198: 199: int index = 0; 200: 201: _my_cmtr_ioptr curr_pfd = pfd; 202: 203: oldnum = (*counter); 204: data32 = big_endian_tag ? _my_htob32(dot->index) : _my_htol32(dot->index); 205: _my_write_cmtr_data(curr_pfd, &data32, 4, *counter); 206: if ((*counter) <= oldnum) { 207: return NULL; 208: } 209: oldnum = (*counter); 210: data32 = big_endian_tag ? _my_htob32(dot->time) : _my_htol32(dot->time); 211: _my_write_cmtr_data(curr_pfd, &data32, 4, *counter); 212: if ((*counter) <= oldnum) { 213: return NULL; 214: } 215: for (index = 0; index < analog_count; index++) { 216: oldnum = (*counter); 217: datatp = (s16)(dot->analogs[index]); 218: data16 = big_endian_tag ? _my_htob16(datatp) : _my_htol16(datatp); 219: _my_write_cmtr_data(curr_pfd, &data16, 2, *counter); 220: if ((*counter) <= oldnum) { 221: return NULL; 222: } 223: } 224: 225: data16 = 0x0000; 226: for (index = 0; index < digit_count; index++) { 227: if (dot->digits[index]) { 228: data16 |= (0x0001 << (index % 16)); 229: } 230: if (((index % 16) == 0) && (index != 0)) { 231: oldnum = (*counter); 232: data16 = big_endian_tag ? _my_htob16(data16) : _my_htol16(data16); 233: _my_write_cmtr_data(curr_pfd, &data16, 2, *counter); 234: if ((*counter) <= oldnum) { 235: return NULL; 236: } 237: } 238: } 239: if (((index % 16) != 1) && (index != 0)) { 240: oldnum = (*counter); 241: data16 = big_endian_tag ? _my_htob16(data16) : _my_htol16(data16); 242: _my_write_cmtr_data(curr_pfd, &data16, 2, *counter); 243: if ((*counter) <= oldnum) { 244: return NULL; 245: } 246: } 247: 248: return curr_pfd; 249: } 250: 251: 252: /* 253: * read_cmtr_cfg_line - 读cmtr配置文件一行,跳过空行. 254: * @pfd: 输入输出参数,地址; 255: * @strline:输出参数,内容缓冲区; 256: * @size:输入参数,缓冲区大小; 257: * @strlend: 输出参数,读取的line的结束字符指针; 258: * @counter: 输出参数,读取的字节计数器; 259: * 260: * 返回当前pfd指针,读失败返回NULL。 261: * 262: */ 263: static _my_cmtr_ioptr read_cmtr_cfg_line(_my_cmtr_ioptr pfd, char *strline, int size, char **strlend,int *counter) 264: { 265: int pos = 0; 266: int oldnum = 0; 267: u8 datard = 0; 268: _my_cmtr_ioptr curr_pfd = pfd; 269: (*strlend) = NULL; 270: if (size <= 0) { 271: return NULL; 272: } 273: if ((!strline) || (!counter)) { 274: return NULL; 275: } 276: 277: memset(strline, '\0', size); 278: while (_my_check_cmtr_ptr(curr_pfd)) { 279: oldnum = (*counter); 280: datard = '\0'; 281: _my_read_cmtr_data(curr_pfd, &datard, 1, *counter); 282: if ((*counter) > oldnum) { 283: if (( datard != '\0') && ( datard != '\r') && ( datard != '\n')) { 284: if ((*strlend) < (strline + size)) { 285: (*(strline + pos)) = datard; 286: (*strlend) = strline + pos; 287: } 288: pos += 1; 289: continue; 290: } else { 291: break; 292: } 293: } 294: } 295: if (pos == 0) { 296: if (_my_check_cmtr_ptr(curr_pfd)) { 297: return read_cmtr_cfg_line(curr_pfd, strline, size, strlend, counter); 298: } else { 299: return NULL; 300: } 301: } 302: 303: return curr_pfd; 304: } 305: 306: /* 307: * read_cmtr_cfg_one_analog - 分析一行模拟量信息. 308: * @analog: 输出参数,模拟量信息; 309: * @strline:输入参数,内容缓冲区; 310: * @size:输入参数,缓冲区大小; 311: * @strlend: 输入参数,读取的line结束指针; 312: * 313: * 返回TRUE成功,失败返回FALSE。 314: * 315: */ 316: static u8 read_cmtr_cfg_one_analog(struct cmtr_cfg_analog *analog, char *strline, int size, char *strlend) 317: { 318: int dataint = 0; 319: f64 dataflt = 0; 320: char* ptrpos = 0; 321: char* strlcur = strline; 322: 323: ptrpos = strchr(strlcur, ','); 324: if ((!ptrpos) || (ptrpos >= strlend)) { 325: return FALSE; 326: } 327: if (sscanf(strlcur, "%d%*s", &dataint) <= 0) { 328: return FALSE; 329: } 330: analog->index = dataint; 331: 332: strlcur = ptrpos += 1; 333: ptrpos = strchr(strlcur, ','); 334: if ((!ptrpos) || (ptrpos >= strlend)) { 335: return FALSE; 336: } 337: memset(analog->name, '\0', CMTR_STRING_MAX_LENGTH); 338: memcpy(analog->name, strlcur, ptrpos-strlcur); 339: 340: strlcur = ptrpos += 1; 341: ptrpos = strchr(strlcur, ','); 342: if ((!ptrpos) || (ptrpos >= strlend)) { 343: return FALSE; 344: } 345: memset(analog->phase, '\0', CMTR_STRING_MAX_LENGTH); 346: memcpy(analog->phase, strlcur, ptrpos-strlcur); 347: 348: strlcur = ptrpos += 1; 349: ptrpos = strchr(strlcur, ','); 350: if ((!ptrpos) || (ptrpos >= strlend)) { 351: return FALSE; 352: } 353: memset(analog->element, '\0', CMTR_STRING_MAX_LENGTH); 354: memcpy(analog->element, strlcur, ptrpos-strlcur); 355: 356: strlcur = ptrpos += 1; 357: ptrpos = strchr(strlcur, ','); 358: if ((!ptrpos) || (ptrpos >= strlend)) { 359: return FALSE; 360: } 361: memset(analog->unit, '\0', CMTR_STRING_MAX_LENGTH); 362: memcpy(analog->unit, strlcur, ptrpos-strlcur); 363: 364: strlcur = ptrpos += 1; 365: ptrpos = strchr(strlcur, ','); 366: if ((!ptrpos) || (ptrpos >= strlend)) { 367: return FALSE; 368: } 369: if (sscanf(strlcur, "%lf%*s", &dataflt) <= 0) { 370: return FALSE; 371: } 372: analog->factor_a = dataflt; 373: 374: strlcur = ptrpos += 1; 375: ptrpos = strchr(strlcur, ','); 376: if ((!ptrpos) || (ptrpos >= strlend)) { 377: return FALSE; 378: } 379: if (sscanf(strlcur, "%lf%*s", &dataflt) <= 0) { 380: return FALSE; 381: } 382: analog->factor_b = dataflt; 383: 384: strlcur = ptrpos += 1; 385: ptrpos = strchr(strlcur, ','); 386: if ((!ptrpos) || (ptrpos >= strlend)) { 387: return FALSE; 388: } 389: if (sscanf(strlcur, "%d%*s", &dataint) <= 0) { 390: return FALSE; 391: } 392: analog->offset_time = dataint; 393: 394: strlcur = ptrpos += 1; 395: ptrpos = strchr(strlcur, ','); 396: if ((!ptrpos) || (ptrpos >= strlend)) { 397: return FALSE; 398: } 399: if (sscanf(strlcur, "%d%*s", &dataint) <= 0) { 400: return FALSE; 401: } 402: analog->smp_min = dataint; 403: 404: strlcur = ptrpos += 1; 405: if (ptrpos > strlend) { 406: return FALSE; 407: } 408: if (sscanf(strlcur, "%d%*s", &dataint) <= 0) { 409: return FALSE; 410: } 411: analog->smp_max = dataint; 412: 413: return TRUE; 414: } 415: 416: /* 417: * read_cmtr_cfg_one_digit - 分析一行数字量信息. 418: * @analog: 输出参数,数字量信息; 419: * @strline:输入参数,内容缓冲区; 420: * @size:输入参数,缓冲区大小; 421: * @strlend: 输入参数,读取的line结束指针; 422: * 423: * 返回TRUE成功,失败返回FALSE。 424: * 425: */ 426: static u8 read_cmtr_cfg_one_digit(struct cmtr_cfg_digit *digit, char *strline, int size, char *strlend) 427: { 428: int dataint = 0; 429: char* ptrpos = 0; 430: char* strlcur = strline; 431: ptrpos = strchr(strlcur, ','); 432: if ((!ptrpos) || (ptrpos >= strlend)) { 433: return FALSE; 434: } 435: if (sscanf(strlcur, "%d%*s", &dataint) <= 0) { 436: return FALSE; 437: } 438: digit->index = dataint; 439: 440: strlcur = ptrpos += 1; 441: ptrpos = strchr(strlcur, ','); 442: if ((!ptrpos) || (ptrpos >= strlend)) { 443: return FALSE; 444: } 445: memset(digit->name, '\0', CMTR_STRING_MAX_LENGTH); 446: memcpy(digit->name, strlcur, ptrpos-strlcur); 447: 448: strlcur = ptrpos += 1; 449: if (ptrpos > strlend) { 450: return FALSE; 451: } 452: if (sscanf(strlcur, "%d", &dataint) <= 0) { 453: return FALSE; 454: } 455: digit->state = dataint; 456: 457: return TRUE; 458: } 459: 460: /* 461: * read_cmtr_cfg_info - 读cmtr配置文件. 462: * @pfd: 输入输出参数,地址 463: * @cfg:输出参数,cmtr(cfg文件)结构体 464: * @counter: 输出参数,读取的字节计数器; 465: * 466: * 返回当前pfd指针,读失败返回NULL 467: * 468: */ 469: _my_cmtr_ioptr read_cmtr_cfg_info(_my_cmtr_ioptr pfd, struct cmtr_cfg_info *cfg, int* counter) 470: { 471: u8 returntag = FALSE; 472: 473: int strsize = 1024; 474: char* strline = NULL; 475: char* strlcur = NULL; 476: char* strlend = NULL; 477: _my_cmtr_ioptr curr_pfd = pfd; 478: 479: char* ptrpos = 0; 480: 481: int port1num = 0; 482: int port2num = 0; 483: char port1chr = '\0'; 484: char port2chr = '\0'; 485: 486: int index = 0; 487: int dataint = 0; 488: f32 dataflt = 0; 489: 490: _my_assert(pfd && cfg); 491: 492: strline = _my_buf_malloc(strsize); 493: memset(strline, '\0', strsize); 494: 495: /* read station name and kymograph id */ 496: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 497: 498: if (!curr_pfd) { 499: goto RETURN_FINISH; 500: } 501: strlcur = strline; 502: ptrpos = strchr(strlcur, ','); 503: memset(cfg->station_name, '\0', CMTR_STRING_MAX_LENGTH); 504: memset(cfg->kymograph_id, '\0', CMTR_STRING_MAX_LENGTH); 505: if (ptrpos) { 506: memcpy(cfg->station_name, strlcur, ptrpos - strlcur); 507: if (ptrpos < strlend) { 508: ptrpos += 1; 509: memcpy(cfg->kymograph_id, ptrpos, strlen(ptrpos)); 510: } 511: } else { 512: memcpy(cfg->station_name, strlcur, strlen(strlcur)); 513: } 514: 515: /* read port count */ 516: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 517: if (!curr_pfd) { 518: goto RETURN_FINISH; 519: } 520: strlcur = strline; 521: ptrpos = strchr(strlcur, ','); 522: if ((!ptrpos) || (ptrpos >= strlend)) { 523: goto RETURN_FINISH; 524: } 525: ptrpos += 1; 526: if (sscanf(ptrpos, "%d%c%*s", &port1num, &port1chr) <= 0) { 527: goto RETURN_FINISH; 528: } 529: ptrpos = strchr(ptrpos, ','); 530: if ((!ptrpos) || (ptrpos >= strlend)) { 531: goto RETURN_FINISH; 532: } 533: ptrpos += 1; 534: if (sscanf(ptrpos, "%d%c", &port2num, &port2chr) <= 0) { 535: goto RETURN_FINISH; 536: } 537: if ((port1chr == 'A') || (port1chr == 'a')) { 538: cfg->analog_count = port1num; 539: cfg->digit_count = port2num; 540: } else if ((port1chr == 'D') || (port1chr == 'd')) { 541: cfg->analog_count = port2num; 542: cfg->digit_count = port1num; 543: } else { 544: goto RETURN_FINISH; 545: } 546: 547: /* read analog port info */ 548: for (index = 0; index < cfg->analog_count; index++) { 549: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 550: if (!curr_pfd) { 551: goto RETURN_FINISH; 552: } 553: if (!read_cmtr_cfg_one_analog((cfg->analogs)+index, strline, strsize, strlend)) { 554: goto RETURN_FINISH; 555: } 556: } 557: /* read digit port info */ 558: for (index = 0; index < cfg->digit_count; index++) { 559: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 560: if (!curr_pfd) { 561: goto RETURN_FINISH; 562: } 563: if (!read_cmtr_cfg_one_digit((cfg->digits)+index, strline, strsize, strlend)) { 564: goto RETURN_FINISH; 565: } 566: } 567: 568: /* read Frequency info */ 569: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 570: if (!curr_pfd) { 571: goto RETURN_FINISH; 572: } 573: if (strline > strlend) { 574: goto RETURN_FINISH; 575: } 576: if (sscanf(strline, "%f", &dataflt) <= 0) { 577: goto RETURN_FINISH; 578: } 579: cfg->frequency = dataflt; 580: 581: /* read smprate count info */ 582: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 583: if (!curr_pfd) { 584: goto RETURN_FINISH; 585: } 586: if (strline > strlend) { 587: goto RETURN_FINISH; 588: } 589: if (sscanf(strline, "%d", &dataint) <= 0) { 590: goto RETURN_FINISH; 591: } 592: cfg->smprate_count = dataint; 593: 594: /* read smprate info */ 595: for (index = 0; index < cfg->smprate_count; index ++) { 596: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 597: if (!curr_pfd) { 598: goto RETURN_FINISH; 599: } 600: strlcur = strline; 601: ptrpos = strchr(strlcur, ','); 602: if ((!ptrpos) || (ptrpos >= strlend)) { 603: goto RETURN_FINISH; 604: } 605: if (sscanf(strlcur, "%f%*s", &dataflt) <= 0) { 606: goto RETURN_FINISH; 607: } 608: cfg->smprates[index].rate = dataflt; 609: strlcur = ptrpos += 1; 610: if (ptrpos > strlend) { 611: goto RETURN_FINISH; 612: } 613: if (sscanf(strlcur, "%d", &dataint) <= 0) { 614: goto RETURN_FINISH; 615: } 616: cfg->smprates[index].point = dataint; 617: } 618: 619: /* read time info */ 620: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 621: if (!curr_pfd) { 622: goto RETURN_FINISH; 623: } 624: memset(cfg->begin_time, '\0', CMTR_STRING_MAX_LENGTH); 625: if (strlend - strline >= 0) { 626: memcpy(cfg->begin_time, strline, strlend - strline + 1); 627: } 628: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 629: if (!curr_pfd) { 630: goto RETURN_FINISH; 631: } 632: memset(cfg->end_time, '\0', CMTR_STRING_MAX_LENGTH); 633: if (strlend - strline >= 0) { 634: memcpy(cfg->end_time, strline, strlend - strline + 1); 635: } 636: 637: /* read dat type info */ 638: curr_pfd = read_cmtr_cfg_line(curr_pfd, strline, strsize, &strlend, counter); 639: if (!curr_pfd) { 640: goto RETURN_FINISH; 641: } 642: strlcur =_my_strupr(strline); 643: ptrpos = strstr(strlcur, "BIN"); 644: memset(cfg->file_type, '\0', CMTR_STRING_MAX_LENGTH); 645: if (ptrpos > 0) { 646: strcpy(cfg->file_type, "BINARY"); 647: } else if (strstr(strlcur, "ASC") > 0) { 648: strcpy(cfg->file_type, "ASCII"); 649: } 650: else { 651: goto RETURN_FINISH; 652: } 653: 654: returntag = TRUE; 655: 656: RETURN_FINISH: 657: _my_buf_free(strline); 658: return returntag ? curr_pfd : NULL; 659: } 660: 661: /* 662: * read_cmtr_dat_smpdot_binary - 读bin格式cmtr采样点数据信息. 663: * @pfd: 输入输出参数,地址 664: * @big_endian_tag: 输入参数,标识文件中数据的字节顺序,为FALSE按照小端读(默认值),True按照大端读; 665: * @analog_count: 输入参数,模拟量个数; 666: * @digit_count: 输入参数,数字量个数; 667: * @dot: 输出参数,采样点信息; 668: * @the_smp_mins: 输出参数,统计最小值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 669: * @the_smp_maxs: 输出参数,统计最大值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 670: * @counter: 输出参数,读取的字节计数器; 671: * 672: * 返回当前pfd指针,读失败返回NULL 673: * 674: */ 675: _my_cmtr_ioptr read_cmtr_dat_smpdot_binary(_my_cmtr_ioptr pfd, u8 big_endian_tag, 676: int analog_count, int digit_count, 677: struct cmtr_dat_smpdot *dot, 678: u32* the_smp_mins, u32* the_smp_maxs, 679: int* counter) 680: { 681: u16 data16 = 0x00; 682: u32 data32 = 0x00; 683: int oldnum = 0; 684: 685: int index = 0; 686: int bitpos = 0; 687: 688: _my_cmtr_ioptr curr_pfd = pfd; 689: 690: oldnum = (*counter); 691: data32 = 0x00; 692: _my_read_cmtr_data(curr_pfd, &data32, 4, *counter); 693: if ((*counter) <= oldnum) { 694: return NULL; 695: } 696: dot->index = big_endian_tag ? _my_btoh32(data32) : _my_ltoh32(data32); 697: 698: oldnum = (*counter); 699: data32 = 0x00; 700: _my_read_cmtr_data(curr_pfd, &data32, 4, *counter); 701: if ((*counter) <= oldnum) { 702: return NULL; 703: } 704: dot->time = big_endian_tag ? _my_btoh32(data32) : _my_ltoh32(data32); 705: 706: /* read analog port info */ 707: for (index = 0; index < analog_count; index ++) { 708: oldnum = (*counter); 709: data16 = 0x00; 710: _my_read_cmtr_data(curr_pfd, &data16, 2, *counter); 711: if ((*counter) <= oldnum) { 712: return NULL; 713: } 714: dot->analogs[index] = big_endian_tag ? _my_btoh16(data16) : _my_ltoh16(data16); 715: if (the_smp_mins) { 716: if (the_smp_mins + index) { 717: the_smp_mins[index] = _my_min(the_smp_mins[index], dot->analogs[index]); 718: } 719: } 720: if (the_smp_maxs) { 721: if (the_smp_maxs + index) { 722: the_smp_maxs[index] = _my_min(the_smp_maxs[index], dot->analogs[index]); 723: } 724: } 725: } 726: /* read digit port info */ 727: for (index = 0; index < digit_count; index ++) { 728: oldnum = (*counter); 729: data16 = 0x00; 730: _my_read_cmtr_data(curr_pfd, &data16, 2, *counter); 731: if ((*counter) <= oldnum) { 732: return NULL; 733: } 734: data16 = big_endian_tag ? _my_btoh16(data16) : _my_ltoh16(data16); 735: for (bitpos = 0; bitpos < 16; bitpos++) { 736: if ((index * 16 + bitpos) < digit_count) { 737: dot->digits[index * 16 + bitpos] = (data16 & (0x0001 << bitpos)); 738: } else { 739: break; 740: } 741: } 742: } 743: 744: return curr_pfd; 745: } 746: 747: /* 748: * read_cmtr_dat_smpdot_ascii - 读ascii格式cmtr采样点数据信息. 749: * @pfd: 输入输出参数,地址 750: * @read_buf: 输入参数,读缓冲,在外部申请内存,避免在内部频繁申请、释放内存; 751: * @buf_size: 输入参数,读缓冲区的大小; 752: * @analog_count: 输入参数,模拟量个数; 753: * @digit_count: 输入参数,数字量个数; 754: * @dot: 输出参数,采样点信息; 755: * @the_smp_mins: 输出参数,统计最小值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 756: * @the_smp_maxs: 输出参数,统计最大值,为数组,个数至少为analog_count个,值为NULL时忽略统计; 757: * @counter: 输出参数,读取的字节计数器; 758: * 759: * 返回当前pfd指针,读失败返回NULL 760: * 761: */ 762: _my_cmtr_ioptr read_cmtr_dat_smpdot_ascii(_my_cmtr_ioptr pfd, 763: u8 *read_buf, int buf_size, 764: int analog_count, int digit_count, 765: struct cmtr_dat_smpdot *dot, 766: u16* the_smp_mins,u16* the_smp_maxs, 767: int* counter) 768: { 769: char* ptrpos = 0; 770: char* strlend = 0; 771: 772: int index = 0; 773: 774: int dataint = 0; 775: float dataflt = 0; 776: 777: _my_cmtr_ioptr curr_pfd = pfd; 778: 779: memset(read_buf, '\0', buf_size); 780: curr_pfd = read_cmtr_cfg_line(curr_pfd, read_buf, buf_size, &strlend, counter); 781: if (!curr_pfd) { 782: return NULL; 783: } 784: 785: ptrpos = read_buf; 786: if ((!ptrpos) || (ptrpos >= strlend)) { 787: return NULL; 788: } 789: if (sscanf(ptrpos, "%d%*s", &dataint) <= 0) { 790: return NULL; 791: } 792: dot->index = dataint; 793: ptrpos = strchr(ptrpos, ','); 794: if (!ptrpos) { 795: return NULL; 796: } 797: ptrpos += 1; 798: if ((!ptrpos) || (ptrpos > strlend)) { 799: return NULL; 800: } 801: if (sscanf(ptrpos, "%d%*s", &dataint) <= 0) { 802: return NULL; 803: } 804: dot->time = dataint; 805: 806: /* read analog port info */ 807: for (index = 0; index < analog_count; index ++) { 808: ptrpos = strchr(ptrpos, ','); 809: if (!ptrpos) { 810: return NULL; 811: } 812: ptrpos += 1; 813: if ((!ptrpos) || (ptrpos > strlend)) { 814: return NULL; 815: } 816: if (sscanf(ptrpos, "%f%*s", &dataflt) <= 0) { 817: return NULL; 818: } 819: dot->analogs[index] = dataflt; 820: if (the_smp_mins) { 821: if (the_smp_mins + index) { 822: the_smp_mins[index] = _my_min(the_smp_mins[index], dataflt); 823: } 824: } 825: if (the_smp_maxs) { 826: if (the_smp_maxs + index) { 827: the_smp_maxs[index] = _my_min(the_smp_maxs[index], dataflt); 828: } 829: } 830: } 831: /* read digit port info */ 832: for (index = 0; index < digit_count; index ++) { 833: ptrpos = strchr(ptrpos, ','); 834: if (!ptrpos) { 835: return NULL; 836: } 837: ptrpos += 1; 838: if ((!ptrpos) || (ptrpos > strlend)) { 839: return NULL; 840: } 841: if (sscanf(ptrpos, "%d%*s", &dataint) <= 0) { 842: return NULL; 843: } 844: dot->digits[index] = dataint; 845: } 846: 847: return curr_pfd; 848: } 849: 850: /* 851: * print_cmtr_cfg_info - 打印cmtr文件配置数. 852: * @cfg: 输入参数,cmtr文件配置数据; 853: * 854: */ 855: void print_cmtr_cfg_info(struct cmtr_cfg_info *cfg) 856: { 857: u32 index = 0; 858: if ( cfg ) { 859: _my_printf("=====================\n"); 860: _my_printf("%s,%s\n", cfg->station_name, cfg->kymograph_id); 861: _my_printf("%d,%dA,%dD\n", cfg->analog_count + cfg->digit_count, cfg->analog_count, cfg->digit_count); 862: for (index = 0; index < cfg->analog_count; index++) { 863: _my_printf("%d,%s,%s,%s,%s,%.6lf,%.6lf,%d,%d,%d\n", 864: cfg->analogs[index].index, 865: cfg->analogs[index].name, 866: cfg->analogs[index].phase, 867: cfg->analogs[index].element, 868: cfg->analogs[index].unit, 869: cfg->analogs[index].factor_a, 870: cfg->analogs[index].factor_b, 871: cfg->analogs[index].offset_time, 872: cfg->analogs[index].smp_min, 873: cfg->analogs[index].smp_max); 874: } 875: for (index = 0; index < cfg->digit_count; index++) { 876: _my_printf("%d,%s,%d\n", 877: cfg->digits[index].index, 878: cfg->digits[index].name, 879: cfg->digits[index].state); 880: } 881: _my_printf("%.6f\n", cfg->frequency); 882: for (index = 0; index < cfg->smprate_count; index++) { 883: _my_printf("%f,%d\n", cfg->smprates[index].rate, cfg->smprates[index].point); 884: } 885: _my_printf("%s\n", cfg->begin_time); 886: _my_printf("%s\n", cfg->end_time); 887: _my_printf("%s\n", cfg->file_type); 888: _my_printf("=====================\n"); 889: } 890: } 891: 892: /* 893: * print_cmtr_dat_smpdot - 打印cmtr数据文件采样点. 894: * @cfg: 输入参数,cmtr文件配置数据; 895: * @analog_count: 输入参数,模拟量个数; 896: * @digit_count: 输入参数,数字量个数; 897: * @dot: 输入参数,采样点信息; 898: * 899: */ 900: void print_cmtr_dat_smpdot(int analog_count, int digit_count, struct cmtr_dat_smpdot *dot) 901: { 902: int index = 0; 903: if ( dot ) { 904: _my_printf("%u,%d", dot->index, dot->time); 905: for (index = 0; index < analog_count; index++) { 906: _my_printf(",%d", dot->analogs[index]); 907: } 908: for (index = 0; index < digit_count; index++) { 909: _my_printf(",%d", dot->digits[index]); 910: } 911: _my_printf("\n"); 912: } 913: } 914: 915: 916: #ifdef CMTR_CONSOLE_DEMO 917: 918: 919: /* 920: * _my_cmtr_openrb/_my_cmtr_openwb/_my_cmtr_openab/_my_cmtr_close 921: * - 可以根据需要改写该接口,如重定义为网口的recv\send、串口r\w等 922: * @pfd: 文件地址、缓冲区地址、或者socket地址等 923: * @str: 文件路径或者联机ip或者串口地址或者共享内存名称 924: * 925: */ 926: #define _my_cmtr_openrb(str) (open((str), O_RDONLY, S_IWRITE | S_IREAD)) 927: #define _my_cmtr_openwb(str) (open((str), O_CREAT | O_RDWR, S_IWRITE | S_IREAD)) 928: #define _my_cmtr_openab(str) (open((str), O_CREAT | O_APPEND | O_RDWR, S_IWRITE | S_IREAD)) 929: #define _my_cmtr_close(pfd) (close(pfd)) 930: 931: 932: /* 933: * cmtr_test_demo - 测试函数 934: * 935: */ 936: 937: void cmtr_test_demo() 938: { 939: int counter1 = 0; 940: int counter2 = 0; 941: struct cmtr_dat_smpdot dot0; 942: struct cmtr_cfg_info cfg0; 943: _my_cmtr_ioptr fdcfg1 = 0; 944: _my_cmtr_ioptr fddat1 = 0; 945: _my_cmtr_ioptr fdcfg2 = 0; 946: _my_cmtr_ioptr fddat2 = 0; 947: int buf_size = 1024; 948: u8 *read_buf = _my_buf_malloc(buf_size); 949: int analog_count = 0; 950: int digit_count = 0; 951: u16 *the_smp_mins = 0; 952: u16 *the_smp_maxs = 0; 953: 954: fdcfg1 = _my_cmtr_openrb("E:/iec-61850/test_r.cfg"); 955: fdcfg2 = _my_cmtr_openwb("E:/iec-61850/test_w.cfg"); 956: _my_assert(fdcfg1); 957: _my_assert(fdcfg2); 958: read_cmtr_cfg_info(fdcfg1, &cfg0, &counter1); 959: print_cmtr_cfg_info(&cfg0); 960: write_cmtr_cfg_info(fdcfg2, &cfg0, &counter2); 961: _my_cmtr_close(fdcfg1); 962: _my_cmtr_close(fdcfg2); 963: 964: 965: fddat1 = _my_cmtr_openrb("E:/iec-61850/test_r.dat"); 966: fddat2 = _my_cmtr_openwb("E:/iec-61850/test_w.dat"); 967: while (_my_check_cmtr_ptr(fddat1)) { 968: analog_count = cfg0.analog_count; 969: digit_count = cfg0.digit_count; 970: if (analog_count > 0) the_smp_mins = _my_buf_malloc(2*analog_count); 971: if (analog_count > 0) the_smp_maxs = _my_buf_malloc(2*analog_count); 972: fddat1 = read_cmtr_dat_smpdot_ascii(fddat1, 973: read_buf, buf_size, 974: analog_count, digit_count, 975: &dot0, 976: the_smp_mins, the_smp_maxs, 977: &counter1); 978: if (_my_check_cmtr_ptr(fddat1)) { 979: print_cmtr_dat_smpdot(analog_count, digit_count, &dot0); 980: fddat2 = write_cmtr_dat_smpdot_ascii(fddat2, analog_count, digit_count, &dot0, &counter2); 981: } 982: } 983: _my_cmtr_close(fddat1); 984: _my_cmtr_close(fddat2); 985: } 986: 987: 988: /* 989: * main - 测试main函数 990: * 991: */ 992: int main(int argc, char* argv[]) 993: { 994: cmtr_test_demo(); 995: getchar(); 996: return 0; 997: } 998: 999: 1000: #endif /* CMTR_CONSOLE_DEMO */ 1001:

最新推荐

recommend-type

二维码工具(1).zip

二维码工具(1).zip
recommend-type

基于ZXing修改的二维码扫描器.zip

基于ZXing修改的二维码扫描器.zip
recommend-type

在网页上生成二维码.zip

在网页上生成二维码.zip
recommend-type

Hyperledger Fabric v2与Accord Project Cicero智能合约开发指南

标题和描述中提到的“hlf-cicero-contract:Accord Project Cicero与Hyperledger Fabric v2签约”以及“半西约合同”暗示了与智能合约和区块链技术相关的知识点。下面详细说明这些知识点: ### 智能合约与区块链技术 智能合约是一套运行在区块链上的程序,当合约条款被触发时,合约会自动执行相应的操作。这种自动执行的特点使得智能合约特别适合于执行多方之间的可信交易,它能减少或消除中介服务的需要,从而降低交易成本并提高效率。 区块链技术是一种分布式账本技术,通过加密算法和共识机制保证了交易数据的不可篡改性和透明性。区块链上的每一笔交易都会被网络中的多个节点验证并记录,确保了交易记录的安全性。 ### Hyperledger Fabric v2 Hyperledger Fabric 是由Linux基金会托管的一个开源项目,它是企业级区块链框架,旨在为商业应用提供安全、模块化、可扩展的区块链平台。Hyperledger Fabric v2.2是该框架的一个版本。 Hyperledger Fabric v2支持链码(Chaincode)概念,链码是部署在Hyperledger Fabric网络上的应用程序,它可以被用来实现各种智能合约逻辑。链码在运行时与网络中的背书节点和排序服务交互,负责验证、执行交易以及维护账本状态。 ### Accord Project Cicero Accord Project Cicero 是一个开源的智能合同模板和执行引擎,它允许开发者使用自然语言来定义合同条款,并将这些合同转换为可以在区块链上执行的智能合约。CiceroMark是基于Markdown格式的一种扩展,它允许在文档中嵌入智能合约逻辑。 通过Accord Project Cicero,可以创建出易于理解、可执行的智能合约。这些合同可以与Hyperledger Fabric集成,利用其提供的安全、透明的区块链网络环境,从而使得合同条款的执行更加可靠。 ### 智能合约的安装与部署 描述中提到了“安装”和“启动”的步骤,这意味着为了使用HLF v2.2和Accord Project Cicero,需要先进行一系列的配置和安装工作。这通常包括设置环境变量(例如HLF_INSTALL_DIR)、安装区块链网络(Test-Net)以及安装其他必需的软件工具(如jq)。 jq是一个轻量级且灵活的命令行JSON处理器,常用于处理JSON数据。在区块链项目中,jq可以帮助开发者处理链码或智能合约的数据,特别是在与网络节点交互时。 ### JavaScript 标签 标签“JavaScript”表明本项目或相关文档中会涉及到JavaScript编程语言。Hyperledger Fabric v2支持多种智能合约语言,其中JavaScript是一个广泛使用的选项。JavaScript在编写链码时提供了灵活的语法和强大的库支持,是进行区块链开发的一个流行选择。 ### 文件结构 文件名称列表“hlf-cicero-contract-master”暗示这是一个包含所有相关文件和资源的项目源代码目录。这个名称通常表明开发者可以从该目录开始探索、安装和配置项目的所有组件。 ### 综合知识点 1. 智能合约与区块链技术可以自动化执行多方面可信交易,降低交易成本并提高效率。 2. Hyperledger Fabric v2为企业提供一个安全、模块化、可扩展的区块链平台。 3. Accord Project Cicero结合Markdown格式的CiceroMark,将自然语言定义的合同条款转换为可执行的智能合约。 4. 智能合约的安装和部署涉及到一系列配置步骤,包括设置环境变量、安装必要的软件工具等。 5. JavaScript是编写智能合约的流行语言之一,为区块链开发提供灵活性和强大的库支持。 6. 项目文件结构表明从一个主目录可以探索和配置所有项目相关文件和资源。 以上是根据文件标题、描述、标签以及文件名称列表所提取的相关知识点。这些知识点对于理解Hyperledger Fabric v2和Accord Project Cicero在智能合约中的应用尤为重要。
recommend-type

深度神经网络优化技巧全解析

# 深度神经网络优化技巧全解析 ## 1 超越 Sigmoid 函数 在神经网络中,Sigmoid 函数曾被广泛使用,但它存在一些问题,如梯度消失。研究人员一直在寻找更好的激活函数,且不同的激活函数在不同场景下表现各异。 ### 1.1 ReLU 函数登场 如今,修正线性单元(ReLU)成为了 Sigmoid 的常用替代函数。与 Sigmoid 相比,ReLU 非常简单,以下是其 Python 实现: ```python def relu(z): if z <= 0: return 0 else: return z ``` ReLU 由两段直
recommend-type

什么是噪声功率密度

<think> 嗯,用户这次问的是噪声功率密度的定义,结合之前的系统指令和站内引用,需要给出清晰准确的中文解释。用户可能是电子工程或通信专业的学生,正在学习噪声相关的理论基础,也可能是工程师需要实际应用概念。从引用来[^1]看,用户对相位噪声的定义有基础认知,但需要系统梳理核心概念。 噪声功率密度最本质的定义应该是单位带宽内的噪声功率。引用[^2]提到电阻热噪声的例子很典型,50Ω电阻在300K环境下功率谱密度是定值(0.9nV/√Hz)²,这说明: 1. 它描述的是噪声功率随频率分布的密度 2. 与带宽强相关(带宽加倍则总功率加倍) 3. 单位通常用W/Hz或V²/Hz 维纳-辛钦定理(
recommend-type

Libshare: Salesforce的高效可重用模块集合

Salesforce是一个云基础的CRM平台,它允许用户构建定制应用程序来满足特定的业务需求。Apex是Salesforce平台上的一个强类型编程语言,用于开发复杂的业务逻辑,通过触发器、类和组件等实现。这些组件使得开发者可以更高效地构建应用程序和扩展Salesforce的功能。 在提到的"libshare:经过测试的Salesforce可重用模块"文件中,首先介绍了一个名为Libshare的工具包。这个工具包包含了一系列已经过测试的可重用模块,旨在简化和加速Salesforce应用程序的开发。 Libshare的各个组成部分的知识点如下: 1. 设置模块:在Salesforce应用程序中,应用程序设置的管理是必不可少的一部分。设置模块提供了一种简便的方式存储应用程序的设置,并提供了一个易用的API来与之交互。这样,开发者可以轻松地为不同的环境配置相同的设置,并且可以快速地访问和修改这些配置。 2. Fluent断言模块:断言是单元测试中的关键组成部分,它们用于验证代码在特定条件下是否表现预期。Fluent断言模块受到Java世界中Assertj的启发,提供了一种更流畅的方式来编写断言。通过这种断言方式,可以编写更易于阅读和维护的测试代码,提高开发效率和测试质量。 3. 秒表模块:在性能调优和效率测试中,记录方法的执行时间是常见的需求。秒表模块为开发者提供了一种方便的方式来记录总时间,并跟踪每种方法所花费的时间。这使得开发者能够识别瓶颈并优化代码性能。 4. JsonMapper模块:随着Web API的广泛应用,JSON数据格式在应用程序开发中扮演了重要角色。JsonMapper模块为开发者提供了一个更高级别的抽象,用于读取和创建JSON内容。这能够大幅简化与JSON数据交互的代码,并提高开发效率。 5. utils模块:在软件开发过程中,经常会遇到需要重复实现一些功能的情况,这些功能可能是通用的,例如日期处理、字符串操作等。utils模块提供了一系列已经编写好的实用工具函数,可以用于节省时间,避免重复劳动,提高开发效率。 6. 记录器模块:记录器通常用于记录应用程序的运行日志,以便于问题诊断和性能监控。系统提供的System.debug功能虽然强大,但在大型应用中,统一的记录器包装器可以使得日志管理更加高效。记录器模块支持记录器名称,并且可以对日志进行适当的封装。 7. App Logger模块:App Logger模块扩展了记录器模块的功能,它允许开发者将日志语句保存到一个精心设计的App Log对象中。此外,App Logger模块支持存储长达56k字符的日志内容,这对于复杂应用的监控和调试非常有用。 8. 应用程序任务模块:在处理异步作业时,例如批量数据处理或定时任务,需要有一个框架来管理和跟踪这些任务。应用程序任务模块提供了一个框架,用于处理可排队的作业,并能够跟踪这些任务的执行情况。 通过Libshare提供的这些模块,Salesforce的开发者能够减少开发工作量,加快开发速度,并提高代码质量。这些模块能够帮助开发者避免重复的“造轮子”工作,专注于核心业务逻辑的实现。同时,由于Libshare作为托管程序包发布,开发者无需担心代码的维护和管理,只需将其添加到自己的Salesforce组织中即可使用。 Libshare的发布也强调了可重用性的重要性,这是软件工程领域中长期提倡的一个原则。通过使用可重用的组件,开发者能够遵循DRY(Don't Repeat Yourself)原则,从而减少代码的冗余,提高生产效率,同时降低因重复编写相同代码而导致错误的风险。 总之,Libshare是一个有价值的资源,对于那些希望在Salesforce平台上快速构建高效、可靠应用程序的开发者来说,这些预置的、经过测试的模块无疑是一个强大的助手。
recommend-type

机器学习技术要点与应用解析

# 机器学习技术要点与应用解析 ## 1. 机器学习基础概念 ### 1.1 数据类型与表示 在编程中,数据类型起着关键作用。Python 具有动态类型特性,允许变量在运行时改变类型。常见的数据类型转换函数包括 `bool()`、`int()`、`str()` 等。例如,`bool()` 函数可将值转换为布尔类型,`int()` 用于将值转换为整数类型。数据类型还包括列表(`lists`)、字典(`dictionaries`)、元组(`tuples`)等集合类型,其中列表使用方括号 `[]` 表示,字典使用花括号 `{}` 表示,元组使用圆括号 `()` 表示。 ### 1.2 变量与命名
recommend-type

点击歌曲没反应

<think> 我们之前已经讨论了如何将音频文件显示在QListView中,并希望实现点击播放。现在遇到的问题是点击列表项没有反应。 原因分析: 1. 我们没有将点击信号连接到播放槽函数。 2. 播放列表(QMediaPlaylist)可能没有正确添加媒体。 3. 媒体路径可能不正确。 解决方案: 按照之前的思路,我们需要做以下几步: 步骤1:确保已经创建了QMediaPlayer和QMediaPlaylist对象,并且播放器设置了播放列表。 步骤2:将QListView的点击信号(clicked)连接到自定义的槽函数,在该槽函数中设置播放列表的当前索引并播放。 步骤3:
recommend-type

SM-CNN-Torch: Torch实现短文本对排名的CNN模型

标题中提到的“SM-CNN-Torch”是一个深度学习框架Torch的实现版本,它基于Severyn和Moschitti在2015年SIGIR会议上发表的一篇论文所描述的卷积神经网络(CNN)模型。这篇论文的内容主要关注的是如何利用CNN对短文本对进行有效的排名,这一点对于问题回答(question-answering, QA)系统来说至关重要。实施该CNN模型的目标是为了更好地处理问答系统中的文本对比较问题,例如,在搜索引擎中确定哪些文档与用户的查询更加相关。 在描述中提到了如何使用该仓库中的代码。首先,用户需要安装Torch库,这是实现和运行SM-CNN-Torch模型的前提条件。接着,用户需要使用提供的脚本(fetch_and_preprocess.sh)下载并预处理GloVe(Global Vectors for Word Representation)字嵌入数据。这一数据集是预先训练好的词向量,能够将单词转换为连续的向量表示,这在深度学习模型中是处理文本的基本步骤。 在模型准备工作中,还需要注意的是Python版本,因为模型运行依赖于Python环境,建议的版本为2.7或更高版本。此外,描述中还提到了并行处理的线程数设置,这表明模型在运行过程中可能会涉及到并行计算,以加速计算过程。通过设置环境变量OMP_NUM_THREADS,可以指定并行计算时的线程数。 文件名称列表中的“SM-CNN-Torch-master”表示这是该仓库的主目录,包含了所有实现Severyn和Moschitti CNN模型的相关文件。 该存储库还包含了一些附加信息,例如,原始Torch实现已经被PyTorch版本所取代。PyTorch是Torch的一个分支,它提供了更多的功能和更易于使用的接口,对研究人员和开发者来说更加友好。此外,该仓库目前仅用于存档目的,这意味着原始的Torch代码不再被积极维护,而是作为一种历史记录保留下来。 标签“deep-learning”表明该项目是一个深度学习项目,所使用的模型是深度神经网络,特别是卷积神经网络。标签“question-answering”则直接指向了问题回答系统,这是深度学习的一个重要应用领域。标签“convolutional-neural-networks”指明了所使用的网络类型是卷积神经网络,它在图像处理和自然语言处理中都有广泛应用。而“Lua”标签则是因为Torch是用Lua语言编写的,尽管它通常与Python一起使用,但也有一个使用Lua的版本。 总的来说,SM-CNN-Torch是一个专门针对短文本对排名的深度学习模型的实现,它允许研究人员和开发者利用已经发表的研究成果来搭建和测试自己的模型,同时为了解其背后原理和实现细节提供了具体的代码和数据处理流程。