活动介绍

ros::NodeHandle pnh("~");转ros2

时间: 2025-07-31 20:42:08 浏览: 13
<think>我们正在将ROS 1的私有节点句柄(ros::NodeHandle pnh("~"))迁移到ROS 2。在ROS 1中,私有节点句柄用于访问节点私有命名空间下的参数、话题和服务。在ROS 2中,节点模型发生了变化,不再使用节点句柄,而是直接通过节点对象来操作。 迁移步骤: 1. 理解ROS 1私有节点句柄的作用 在ROS 1中,使用`ros::NodeHandle pnh("~")`创建的句柄,其命名空间是当前节点的私有命名空间(即节点名后接一个波浪线`~`)。例如,节点名为`/camera_node`,则私有命名空间为`/camera_node/`。在这个命名空间下的参数、话题等都是私有的。 2. ROS 2中的对应概念 ROS 2取消了节点句柄(NodeHandle)的概念,所有操作都通过节点(Node)对象本身进行。在ROS 2中,每个节点都有自己的命名空间,并且可以通过节点的方法直接访问参数、创建发布者/订阅者等。 3. 参数获取的迁移 在ROS 1中,使用私有节点句柄获取参数: ```cpp ros::NodeHandle pnh("~"); int num; pnh.param<int>("num", num, 10); // 从私有命名空间获取参数num ``` 在ROS 2中,使用节点的`declare_parameter`和`get_parameter`方法: ```cpp // 声明参数并设置默认值 this->declare_parameter<int>("num", 10); // 获取参数 int num = this->get_parameter("num").as_int(); ``` 4. 话题和服务的迁移 在ROS 1中,使用私有节点句柄创建发布者/订阅者: ```cpp ros::NodeHandle pnh("~"); ros::Publisher pub = pnh.advertise<std_msgs::String>("topic_name", 10); ``` 在ROS 2中,直接使用节点对象创建,并且话题名称默认是相对于节点命名空间的(类似于ROS 1的私有句柄行为)。但是,如果需要显式指定为私有,可以在话题名称前加上波浪线`~`: ```cpp // 创建发布者,话题名称为私有(相对于节点命名空间) rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub = this->create_publisher<std_msgs::msg::String>("~/topic_name", 10); ``` 注意:在ROS 2中,`~/`开头的名称表示私有名称,会被解析为`/node_namespace/node_name/topic_name`。 5. 完整示例对比 ROS 1节点代码片段: ```cpp #include <ros/ros.h> int main(int argc, char** argv) { ros::init(argc, argv, "my_node"); ros::NodeHandle nh; // 公共句柄 ros::NodeHandle pnh("~"); // 私有句柄 int num; pnh.param<int>("num", num, 10); ros::Publisher pub = pnh.advertise<std_msgs::String>("my_topic", 10); // ... } ``` ROS 2节点代码片段: ```cpp #include <rclcpp/rclcpp.hpp> #include <std_msgs/msg/string.hpp> class MyNode : public rclcpp::Node { public: MyNode() : Node("my_node") { // 声明并获取参数 this->declare_parameter<int>("num", 10); int num = this->get_parameter("num").as_int(); // 创建发布者,话题名为私有(相对于节点命名空间) pub_ = this->create_publisher<std_msgs::msg::String>("~/my_topic", 10); } private: rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<MyNode>()); rclcpp::shutdown(); return 0; } ``` 6. 注意事项: - ROS 2中节点类继承自`rclcpp::Node`,所有操作都在节点类的方法中完成。 - 参数服务:ROS 2提供了更强大的参数机制,支持动态参数和参数回调。 - 命名空间:在ROS 2中,节点的命名空间可以通过启动节点时设置,例如: `ros2 run my_package my_node --ros-args -r __ns:=/my_namespace` 这样,节点私有话题就会变成`/my_namespace/my_node/my_topic`。 7. 总结: ROS 1的私有节点句柄(`ros::NodeHandle pnh("~")`)在ROS 2中被节点自身的方法替代。参数获取使用`declare_parameter`和`get_parameter`,而创建私有话题或服务时,在名称前加上`~/`前缀即可。 引用说明: [^1]: ROS 1的私有节点句柄用于节点的私有命名空间。 [^2]: ROS 2中节点对象直接管理参数和话题。 [^3]: ROS 2的私有话题通过`~/`前缀实现。 相关问题: 1. ROS 2中如何设置节点的命名空间? 2. ROS 2参数服务有哪些新特性? 3. 如何在ROS 2中创建私有服务?</think>### 将 ROS 1 私有节点句柄迁移到 ROS 2 的完整指南 在 ROS 1 中,`ros::NodeHandle pnh("~")` 用于创建私有节点句柄,主要用于访问节点私有命名空间下的参数和资源[^1][^4]。迁移到 ROS 2 后,节点模型发生了根本性变化,不再使用节点句柄概念。以下是关键迁移步骤: #### 1. ROS 2 的节点模型变化 ```diff // ROS 1 (使用节点句柄) - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); // ROS 2 (节点即句柄) + class MyNode : public rclcpp::Node { + // 直接使用 this 访问节点功能 + } ``` ROS 2 取消了显式的节点句柄,节点类本身包含了所有功能[^2][^4]。 #### 2. 参数获取的迁移 **ROS 1 方式**: ```cpp ros::NodeHandle pnh("~"); int param_value; pnh.param("param_name", param_value, 42); // 获取私有参数[^5] ``` **ROS 2 等效实现**: ```cpp class MyNode : public rclcpp::Node { public: MyNode() : Node("my_node") { // 声明并获取参数 this->declare_parameter<int>("param_name", 42); int param_value = this->get_parameter("param_name").as_int(); } }; ``` #### 3. 话题/服务创建的迁移 **ROS 1 私有命名空间话题**: ```cpp ros::NodeHandle pnh("~"); ros::Publisher pub = pnh.advertise<std_msgs::String>("private_topic", 10); ``` **ROS 2 等效实现**: ```cpp class MyNode : public rclcpp::Node { public: MyNode() : Node("my_node") { // 使用 ~/ 前缀表示私有话题 publisher_ = this->create_publisher<std_msgs::msg::String>( "~/private_topic", // ~/ 表示私有命名空间 10 ); } }; ``` #### 4. 命名空间解析对比 | 类型 | ROS 1 (`pnh("~")`) | ROS 2 (`~/`) | |--------------|---------------------------|----------------------------| | 节点名 | `/my_node` | `/my_node` | | 参数路径 | `/my_node/param_name` | `/my_node/param_name` | | 私有话题路径 | `/my_node/private_topic` | `/my_node/private_topic` | | 全局话题路径 | 需显式指定 `/global_topic` | 直接使用 `/global_topic` | #### 5. 启动文件参数配置对比 **ROS 1 launch 文件** ([^5]): ```xml <node name="demo" pkg="demo" type="demo"> <param name="num" value="10" /> <!-- 私有参数 --> </node> ``` **ROS 2 launch 文件等效**: ```python Node( package='demo', executable='demo', name='demo', parameters=[{'num': 10}] # 私有参数直接传递 ) ``` #### 6. 完整迁移示例 ```cpp // ROS 2 节点实现 #include <rclcpp/rclcpp.hpp> #include <std_msgs/msg/string.hpp> class MyNode : public rclcpp::Node { public: MyNode() : Node("my_node") { // 参数处理 this->declare_parameter<int>("processing_rate", 30); int rate = this->get_parameter("processing_rate").as_int(); // 私有话题发布者 (~/ 前缀) private_pub_ = this->create_publisher<std_msgs::msg::String>( "~/internal_data", 10 ); // 全局话题订阅者 global_sub_ = this->create_subscription<std_msgs::msg::String>( "/sensor_data", 10, [this](const std_msgs::msg::String::SharedPtr msg) { // 处理逻辑 } ); } private: rclcpp::Publisher<std_msgs::msg::String>::SharedPtr private_pub_; rclcpp::Subscription<std_msgs::msg::String>::SharedPtr global_sub_; }; ``` #### 关键差异总结 1. **节点模型**:ROS 2 使用面向节点的设计,不再需要显式句柄[^2] 2. **参数访问**:直接通过节点方法访问,无需句柄[^4] 3. **命名空间**: - 私有资源:使用 `~/` 前缀(如 `~/private_topic`) - 全局资源:直接使用绝对路径(如 `/global_topic`) 4. **启动配置**:参数通过 YAML 文件或 launch 文件直接传递 > **最佳实践**:在 ROS 2 中,所有节点私有资源(参数、话题、服务)都通过 `~/` 前缀标识,这直接对应于 ROS 1 的私有节点句柄功能[^3][^4]。
阅读全文

相关推荐

#pragma once #include <ros/ros.h> #include <sensor_msgs/Imu.h> //ACC GYR QUAT #include <sensor_msgs/MagneticField.h> //MAG #include <sensor_msgs/FluidPressure.h> //PR #include <sensor_msgs/Temperature.h> //TEMP #include <geometry_msgs/Vector3Stamped.h> //EUL #include <sys/time.h> #include <signal.h> #include "serial_port.h" #include "hipnuc_lib_package/hipnuc_dec.h" class SerialIMUNode { public: SerialIMUNode(ros::NodeHandle& nh, ros::NodeHandle& pnh); ~SerialIMUNode(); void run(); void open(); void close(); private: static void signal_handler(int sig); void alarm_handler(int sig); void loadParameters(); void initPublishers(); void initSerialPort(); void initAlarm(); void publish_imu_data( hipnuc_raw_t *data, sensor_msgs::Imu *imu_data); void publish_mag_data( hipnuc_raw_t *data, sensor_msgs::MagneticField *mag_data); void publish_eul_data( hipnuc_raw_t *data, geometry_msgs::Vector3Stamped *eul_data); void publish_pre_data( hipnuc_raw_t *data, sensor_msgs::FluidPressure *pre_data); void publish_temp_data(hipnuc_raw_t *data, sensor_msgs::Temperature *temp_data); ros::NodeHandle nh_, pnh_; ros::Publisher imu_pub_, eul_pub_, mag_pub_, pre_pub_, temp_pub_; std::unique_ptr<SerialPort> serial_port_; std::string serial_port_name_; int baud_rate_; std::string frame_id_; std::string imu_topic_, eul_topic_, mag_topic_, pre_topic_, temp_topic_; std::string imu_axes; hipnuc_raw_t raw; const int MAX_RECONNECT_ATTEMPTS = 20; int reconnect_count = 0; int timeout; int timeout_sec; int get_port_timeout_ms = 0; struct itimerval timer; static SerialIMUNode* instance_; sensor_msgs::Imu imu_msg; sensor_msgs::Temperature temp_msg; sensor_msgs::FluidPressure pre_msg; sensor_msgs::MagneticField mag_msg; geometry_msgs::Vector3Stamped eul_msg; bool imu_enable, mag_enable, eul_enable, pre_enable, temp_enable; }; 分析下上述结构

/* * 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中的转向不变但点云在转动,而且小车相对点云位置准确而且稳定

最新推荐

recommend-type

rust-std-static-1.54.0-3.module_el8.5.0+1023+0c63d3d6.tar.gz

# 适用操作系统:Centos8 #Step1、解压 tar -zxvf xxx.el8.tar.gz #Step2、进入解压后的目录,执行安装 sudo rpm -ivh *.rpm
recommend-type

获取本机公网IP发送到飞书群脚本

#获取本机公网IP发送到飞书群 #需要安装jq软件 #需要创建一个飞书群的机器人
recommend-type

SimulinkSF平台下纯电动+四档变速箱动力性与经济性仿真模型及其应用 · 电动汽车 完整版

内容概要:本文介绍了一个基于Simulink/SF平台开发的纯电动加四档变速箱的仿真模型。该模型旨在通过对电动汽车的驱动力和能耗进行精确数据分析,为设计和优化提供参考。文中详细描述了VCU控制模型和换档控制模型的功能,这两个模型分别负责车辆的动力性控制和换档操作,确保汽车在不同速度和负载下获得最佳性能。此外,还涵盖了动力性与经济性仿真的具体应用场景,如最高车速、最大爬坡度、加速时间和能耗续航等方面的模拟。最后,提供了详细的建模及使用说明文档,方便初学者理解和使用。 适合人群:从事新能源汽车研发的专业人士、研究人员及高校相关专业师生。 使用场景及目标:适用于电动汽车的设计与优化,帮助理解车辆性能并为后续改进提供依据。也可作为教学工具,帮助学生掌握电动汽车核心技术。 其他说明:未来将继续优化此模型,以更好地支持新能源汽车的发展。
recommend-type

【超表面】基于matlab生成超表面布局并模拟超透镜作为点源的叠加【含Matlab源码 13984期】.zip

Matlab领域上传的视频是由对应的完整代码运行得来的,完整代码皆可运行,亲测可用,适合小白; 1、从视频里可见完整代码的内容 主函数:main.m; 调用函数:其他m文件;无需运行 运行结果效果图; 2、代码运行版本 Matlab 2019b;若运行有误,根据提示修改;若不会,私信博主; 3、运行操作步骤 步骤一:将所有文件放到Matlab的当前文件夹中; 步骤二:双击打开main.m文件; 步骤三:点击运行,等程序运行完得到结果; 4、仿真咨询 如需其他服务,可私信博主; 4.1 博客或资源的完整代码提供 4.2 期刊或参考文献复现 4.3 Matlab程序定制 4.4 科研合作
recommend-type

西门子S1200与霍尼韦尔电动比例阀PID控制恒温恒压供冷却水系统方案及Eplan电气图纸详解 工业自动化 高效版

内容概要:本文介绍了基于西门子S1200P ID和霍尼韦尔电动比例阀的PID控制系统,旨在实现冷却水的恒温恒压供应。文中详细解释了系统的工作原理,包括PID控制算法的应用、霍尼韦尔电动比例阀对水温的调控机制、两台西门子V20变频器对水压的调控机制,以及整个系统的代码分析框架和Eplan电气图纸的作用。此外,还探讨了该系统在提高生产效率、质量和降低成本方面的好处。 适合人群:从事工业自动化领域的工程师和技术人员,尤其是那些负责冷却水系统的设计、安装和维护的专业人士。 使用场景及目标:适用于需要精确控制冷却水温度和压力的工业环境,如制造业工厂、化工厂等。目标是确保冷却水系统稳定运行,提升生产效率并节约能源。 其他说明:文中提到的具体代码未公开,但提供了详细的代码分析框架,帮助读者理解和实现类似系统。Eplan电气图纸则有助于更好地理解系统架构和维护方法。
recommend-type

GHCN气象站邻接矩阵的Python实现及地理距离应用

根据提供的文件信息,我们可以解析出以下知识点: **标题:“GHCN_邻接矩阵”** 全球历史气候网络(Global Historical Climatology Network,简称GHCN)是一个国际性项目,旨在收集和提供全球范围内的历史气候数据。邻接矩阵(Adjacency Matrix)是图论中的一个概念,用来表示图中各个顶点之间的相邻关系。 **知识点详细说明:** 1. **全球历史气候网络(GHCN):** - GHCN是一个汇集了全球范围内的历史气候数据资料的大型数据库。该数据库主要收集了全球各地的气象站提供的气温、降水、风速等气象数据。 - 这些数据的时间跨度很广,有些甚至可以追溯到19世纪中叶,为气候学家和相关研究人员提供了丰富的气候变迁数据。 - 通过分析这些数据,科学家可以研究气候变化的趋势、模式以及影响因素等。 2. **邻接矩阵:** - 在图论中,邻接矩阵是用来表示图中各个顶点之间相互连接关系的矩阵。 - 无向图的邻接矩阵是一个对称矩阵,如果顶点i与顶点j之间存在一条边,则矩阵中的元素A[i][j]和A[j][i]为1;否则为0。 - 邻接矩阵常用于计算机算法中,比如用于计算最短路径、网络的连通性、以及进行图的遍历等。 3. **地理距离:** - 在这个问题的上下文中,指的是气象站之间的空间距离。 - 计算气象站之间的地理距离通常使用地理信息系统(GIS)或球面几何学的方法,比如使用哈弗辛公式(Haversine formula)计算两个地点之间的大圆距离。 - 通过地理距离数据,可以推断出气候数据在空间分布上的相关性或依赖性。 4. **Python编程语言:** - 标签中提及的Python是一种广泛应用于数据科学、人工智能、网络开发等领域的高级编程语言。 - Python因其易学易用、语法简洁、库支持丰富等特点,在科研、教育、工业界等领域得到广泛应用。 5. **代码实现:** - 提到的代码应该会涉及获取GHCN数据集、计算气象站间的地理距离、以及根据这些距离构建无向图的邻接矩阵。 - 代码可能使用了Python中的科学计算库,如NumPy或SciPy,以及地理计算库,如geopy或Shapely。 - 通过构建邻接矩阵,此代码可以进一步用于分析气候数据的空间分布特征或执行图相关的数据分析任务。 **文件名称列表:“GHCN_Adjacency_Matrix-main”** 文件名“GHCN_Adjacency_Matrix-main”表明这是一个包含主要代码文件的文件夹。在Python中,“main”一词通常表示该文件夹中的某个脚本文件(如“main.py”)是程序的入口点,它是运行程序时最先被调用的文件。 总结以上内容,我们可知该文件描述了一个使用Python语言,通过处理全球历史气候网络(GHCN)中的数据来生成气象站之间地理距离的邻接矩阵的代码。这种邻接矩阵可以用于气候数据分析中的图论应用,如研究气象站点数据的空间分布模式和关系。代码的实现可能用到了多种Python库,旨在为气候研究提供一种新的数据处理和分析视角。
recommend-type

并发编程:多语言实践与策略选择

### 并发编程:多语言实践与策略选择 #### 1. 文件大小计算的并发实现 在并发计算文件大小的场景中,我们可以采用数据流式方法。具体操作如下: - 创建两个 `DataFlowQueue` 实例,一个用于记录活跃的文件访问,另一个用于接收文件和子目录的大小。 - 创建一个 `DefaultPGroup` 来在线程池中运行任务。 ```plaintext graph LR A[创建 DataFlowQueue 实例] --> B[创建 DefaultPGroup] B --> C[执行 findSize 方法] C --> D[执行 findTotalFileS
recommend-type

3utools使用教程

<think>我们注意到用户查询的是"3utools",但引用中提到的都是"uTools"(没有数字3)。这可能是一个拼写错误或版本差异。 根据引用内容,uTools是一个桌面效率工具,提供插件化的功能。因此,我们假设用户指的是uTools,并据此提供使用指南。 如果确实存在名为"3uTools"的软件(例如,苹果设备管理工具),那么我们需要澄清:引用中提到的uTools与3uTools不同。 但是,由于引用中只提到了uTools,且用户的问题中写的是“3utools”(可能是uTools的误写),我们将按照uTools来回答。 如果用户确实指的是另一个软件(3uTools),那么
recommend-type

探索更多视频功能的JavaScript实现

从给定的信息中我们可以提炼出关键词"更多视频"以及"JavaScript",以及一个与文件相关的命名"MoreVideo-master"。接下来,我会针对这些关键词展开详细的IT知识点阐述。 首先,关于“更多视频”,这个描述暗示了我们即将探讨的是与视频内容相关的技术或应用。在现代IT领域中,视频内容的处理、存储、传输和播放是一个非常重要的分支,涉及到的技术包括但不限于视频编码、流媒体技术、网络协议、前端展示技术等。视频内容的增多以及互联网带宽的不断提升,使得在线视频消费成为可能。从最早的ASCII动画到现代的高清视频,技术的演进一直不断推动着我们向更高质量和更多样化的视频内容靠近。 其次,“JavaScript”是IT行业中的一个关键知识点。它是一种广泛使用的脚本语言,特别适用于网页开发。JavaScript可以实现网页上的动态交互,比如表单验证、动画效果、异步数据加载(AJAX)、以及单页应用(SPA)等。作为一种客户端脚本语言,JavaScript可以对用户的输入做出即时反应,无需重新加载页面。此外,JavaScript还可以运行在服务器端(例如Node.js),这进一步拓宽了它的应用范围。 在探讨JavaScript时,不得不提的是Web前端开发。在现代的Web应用开发中,前端开发越来越成为项目的重要组成部分。前端开发人员需要掌握HTML、CSS和JavaScript这三大核心技术。其中,JavaScript负责赋予网页以动态效果,提升用户体验。JavaScript的库和框架也非常丰富,比如jQuery、React、Vue、Angular等,它们可以帮助开发者更加高效地编写和管理前端代码。 最后,关于文件名“MoreVideo-master”,这里的“Master”通常表示这是一个项目或者源代码的主版本。例如,在使用版本控制系统(如Git)时,“Master”分支通常被认为是项目的主分支,包含最新的稳定代码。文件名中的“MoreVideo”表明该项目与视频相关的内容处理功能正在增加或扩展。可能是对现有功能的增强,也可能是为视频播放、视频处理或视频管理增加了新的模块或特性。 综合上述内容,我们可以总结出以下几个IT知识点: 1. 视频技术:包括视频编解码技术、流媒体技术、网络协议、视频格式转换等。在客户端和服务器端,视频技术的应用场景广泛,如在线视频平台、视频会议系统、视频监控系统等。 2. JavaScript应用:JavaScript在Web前端开发中的应用十分广泛,用于实现网页的动态效果和交互性,以及在后端通过Node.js提供服务器端编程能力。 3. 前端开发技术:前端开发不仅仅是页面的静态显示,更重要的是通过JavaScript、CSS和HTML等技术实现用户界面与用户之间的交互。前端框架和库的使用大大提高了开发效率。 4. 版本控制:在IT项目开发过程中,版本控制是一个重要的环节。它帮助开发者管理代码的变更历史,支持多人协作开发,使得代码维护和升级变得更加容易。 通过以上知识点,我们可以看到,无论是“更多视频”的技术应用还是“JavaScript”编程语言的使用,以及文件命名所隐含的项目管理概念,都是IT领域内相互关联且密不可分的几个重要方面。
recommend-type

并发编程中的Clojure代理与相关技术解析

### 并发编程中的Clojure代理与相关技术解析 #### 1. Clojure代理概述 Clojure代理代表内存中的单个标识或位置。与软件事务内存(STM)的引用管理多个标识的协调同步更改不同,代理允许对其管理的单个标识进行独立的异步更改。这些更改以函数或操作的形式表达,并异步应用于该位置。多个独立的并发操作会按顺序依次运行。操作成功完成后,代理将更新为操作返回的新状态,该新状态用于后续对代理的读取或操作。 调用在代理上运行操作的方法会立即返回,操作随后会使用Clojure管理的线程池中的一个线程应用于代理。如果操作主要受CPU限制,可使用`send()`方法运行;如果函数中可能发