time stamp convert tools 1.0.0.1 on vs2010

本文介绍了一款用于将PE文件中的时间戳十六进制值转换为标准日期时间格式的控制台程序。该程序解决了PEview等工具无法直接显示可读时间格式的问题,支持多种输入格式,并提供了源代码。

PEview(v0.99)中有显示时间戳的功能,但是不知道为啥显示不出来.
CFF, studPE只显示时间戳的值,但是没有转成人类识别的时间字符串.
写了一个控制台程序,用来贴入PE工具得到的时间戳的16进制值, 输出时间戳字符串.

工程下载点

src_time_stamp_convert_tools.zip

实验

// test_2017_0529_1356.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <Windows.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

BOOL php_time_2_systemtime(long lPhpTime, SYSTEMTIME* pst);
int get_offset_utc_and_localtime();

/** run result
================================================================================
time stamp convert tools 1.0.0.1 on vs2010
================================================================================
time stamp input value can accept below:
         592BC182
         0x592bc182
         0x592BC182
         0X592BC182
--------------------------------------------------------------------------------
please input time stamp : 0x592bc182
time stamp input is : 0x592BC182
time stamp input convert to time is : 2017-05-29 14:36:50
--------------------------------------------------------------------------------
请按任意键继续. . .
*/

int _tmain(int argc, _TCHAR* argv[])
{
    BOOL bRc = FALSE;
    SYSTEMTIME st;
    char szTime[11] = {'\0'};
    DWORD dwTimeStamp = 0;
    long ltm = 0;

    printf("================================================================================\n");
    printf("time stamp convert tools 1.0.0.1 on vs2010\n");
    printf("================================================================================\n");
    printf("time stamp input value can accept below:\n");
    printf("\t 592BC182\n");
    printf("\t 0x592bc182\n");
    printf("\t 0x592BC182\n");
    printf("\t 0X592BC182\n");
    printf("--------------------------------------------------------------------------------\n");

    printf("please input time stamp : ");
    scanf("%10s", szTime);  // can input 0x592BC182 or 592BC182

    dwTimeStamp = (int)strtol(szTime, NULL, 16);
    printf("time stamp input is : 0x%X\n", dwTimeStamp);
    ltm = (long)dwTimeStamp;

    ltm += get_offset_utc_and_localtime();
    bRc = php_time_2_systemtime(ltm, &st);
    if (bRc) {
        printf("time stamp input convert to time is : %4.4d-%2.2d-%2.2d %2.2d:%2.2d:%2.2d\n",
            st.wYear,
            st.wMonth,
            st.wDay,
            st.wHour,
            st.wMinute,
            st.wSecond);
    } else {
        printf("invalid time stamp input\n");
    }

    printf("--------------------------------------------------------------------------------\n");
    system("pause");
    return 0;
}

int get_offset_utc_and_localtime()
{
    time_t now = 0;
    time_t utc = 0;
    time_t local = 0;
    struct tm* tmutc;
    struct tm* tmlocal;

    time(&now);
    tmutc = gmtime(&now);
    utc = mktime(tmutc);
    tmlocal = localtime(&now);
    local = mktime(tmlocal);

    return (local - utc);
}

BOOL php_time_2_systemtime(long lPhpTime, SYSTEMTIME* pst)
{  
    BOOL        bRc = FALSE;  
    time_t      tmt = -1;  
    struct tm*  pGmt = NULL;  

    do   
    {  
        if (NULL == pst)  
            break;  

        tmt = lPhpTime;  
        pGmt = gmtime(&tmt);

        if (NULL == pGmt)  
            break;  

        pst->wYear = 1900 + pGmt->tm_year;  
        pst->wMonth = 1 + pGmt->tm_mon;  
        pst->wDay = pGmt->tm_mday;  
        pst->wHour = pGmt->tm_hour;  
        pst->wMinute = pGmt->tm_min;  
        pst->wSecond = pGmt->tm_sec;  
        pst->wDayOfWeek = pGmt->tm_wday;  

        bRc = TRUE;  
    } while (0);  

    return bRc;  
}
/* * 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中的转向不变但点云在转动,而且小车相对点云位置准确而且稳定
08-10
ros2中晃动imu,但是我的机器人模型不跟着晃动,而且里程计一开,里程计跑走,但是imu和机器人模型还在原地,以下是我的各个文件代码,情分析一下为什么:wheeltec_robot.cpp文件代码为: #include "turn_on_wheeltec_robot/wheeltec_robot.h" #include "turn_on_wheeltec_robot/Quaternion_Solution.h" #include "wheeltec_robot_msg/msg/data.hpp" sensor_msgs::msg::Imu Mpu6050;//Instantiate an IMU object //实例化IMU对象 using std::placeholders::_1; using namespace std; rclcpp::Node::SharedPtr node_handle = nullptr; //自动回充使用相关变量 bool check_AutoCharge_data = false; bool charge_set_state = false; /************************************** Date: January 28, 2021 Function: The main function, ROS initialization, creates the Robot_control object through the Turn_on_robot class and automatically calls the constructor initialization 功能: 主函数,ROS初始化,通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化 ***************************************/ int main(int argc, char** argv) { rclcpp::init(argc, argv); //ROS initializes and sets the node name //ROS初始化 并设置节点名称 turn_on_robot Robot_Control;//Instantiate an object //实例化一个对象 Robot_Control.Control();//Loop through data collection and publish the topic //循环执行数据采集和发布话题等操作 return 0; } /************************************** Date: January 28, 2021 Function: Data conversion function 功能: 数据转换函数 ***************************************/ short turn_on_robot::IMU_Trans(uint8_t Data_High,uint8_t Data_Low) { short transition_16; transition_16 = 0; transition_16 |= Data_High<<8; transition_16 |= Data_Low; return transition_16; } float turn_on_robot::Odom_Trans(uint8_t Data_High,uint8_t Data_Low) { float data_return; short transition_16; transition_16 = 0; transition_16 |= Data_High<<8; //Get the high 8 bits of data //获取数据的高8位 transition_16 |= Data_Low; //Get the lowest 8 bits of data //获取数据的低8位 data_return = (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s return data_return; } /************************************** Date: January 28, 2021 Function: The speed topic subscription Callback function, according to the subscribed instructions through the serial port command control of the lower computer 功能: 速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令控制下位机 ***************************************/ void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]= 0x06; //frame head 0x7B //帧头0X7B //Send_Data.tx[1] = AutoRecharge; //set aside //预留位 Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 (mm/s) Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot's Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; //(1000*rad/s) Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCC校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; //frame tail 0x7D //帧尾0X7D Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 28, 2021 Function: Publish the IMU data topic 功能: 发布IMU数据话题 ***************************************/ void turn_on_robot::Publish_ImuSensor() { sensor_msgs::msg::Imu Imu_Data_Pub; //Instantiate IMU topic data //实例化IMU话题数据 Imu_Data_Pub.header.stamp = rclcpp::Node::now(); Imu_Data_Pub.header.frame_id = gyro_frame_id; //IMU corresponds to TF coordinates, which is required to use the robot_pose_ekf feature pack //IMU对应TF坐标,使用robot_pose_ekf功能包需要设置此项 Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; //A quaternion represents a three-axis attitude //四元数表达三轴姿态 Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; Imu_Data_Pub.orientation.z = Mpu6050.orientation.z; Imu_Data_Pub.orientation.w = Mpu6050.orientation.w; Imu_Data_Pub.orientation_covariance[0] = 1e6; //Three-axis attitude covariance matrix //三轴姿态协方差矩阵 Imu_Data_Pub.orientation_covariance[4] = 1e6; Imu_Data_Pub.orientation_covariance[8] = 1e-6; Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; //Triaxial angular velocity //三轴角速度 Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y; Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z; Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; //Triaxial angular velocity covariance matrix //三轴角速度协方差矩阵 Imu_Data_Pub.angular_velocity_covariance[4] = 1e6; Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6; Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; //Triaxial acceleration //三轴线性加速度 Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z; imu_publisher->publish(Imu_Data_Pub); //Pub IMU topic //发布IMU话题 } /************************************** Date: January 28, 2021 Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular velocity about triaxial, TF parent-child coordinates, and covariance matrix 功能: 发布里程计话题,包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵 ***************************************/ void turn_on_robot::Publish_Odom() { //Convert the Z-axis rotation Angle into a quaternion for expression //把Z轴转角转换为四元数进行表达 tf2::Quaternion q; q.setRPY(0,0,Mpu6050_Data.imu_yaw); geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q); nav_msgs::msg::Odometry odom; //Instance the odometer topic data //实例化里程计话题数据 odom.header.stamp = rclcpp::Node::now(); ; odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标 odom.pose.pose.position.x = Robot_Pos.X; //Position //位置 odom.pose.pose.position.y = Robot_Pos.Y; //odom.pose.pose.position.z = Robot_Pos.Z; odom.pose.pose.orientation = odom_quat; //Posture, Quaternion converted by Z-axis rotation //姿态,通过Z轴转角转换的四元数 odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标 odom.twist.twist.linear.x = Robot_Vel.X; //Speed in the X direction //X方向速度 //odom.twist.twist.linear.y = Robot_Vel.Y; //Speed in the Y direction //Y方向速度 odom.twist.twist.angular.z = Mpu6050.angular_velocity.z; //Angular velocity around the Z axis //绕Z轴角速度 //There are two types of this matrix, which are used when the robot is at rest and when it is moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature pack //这个矩阵有两种,分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵,用于robot_pose_ekf功能包 //if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0) if(Robot_Vel.X== 0) //If the velocity is zero, it means that the error of the encoder will be relatively small, and the data of the encoder will be considered more reliable //如果velocity是零,说明编码器的误差会比较小,认为编码器数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)), memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2)); else //If the velocity of the trolley is non-zero, considering the sliding error that may be brought by the encoder in motion, the data of IMU is considered to be more reliable //如果小车velocity非零,考虑到运动中编码器可能带来的滑动误差,认为imu的数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)), memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance)); odom_publisher->publish(odom); //Pub odometer topic //发布里程计话题 } /************************************** Date: January 28, 2021 Function: Publish voltage-related information 功能: 发布电压相关信息 ***************************************/ void turn_on_robot::Publish_Voltage() { std_msgs::msg::Float32 voltage_msgs; //Define the data type of the power supply voltage publishing topic //定义电源电压发布话题的数据类型 static float Count_Voltage_Pub=0; if(Count_Voltage_Pub++>10) { Count_Voltage_Pub=0; voltage_msgs.data = Power_voltage; //The power supply voltage is obtained //电源供电的电压获取 voltage_publisher->publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位:V、伏特 } } ////////// 回充发布与回调 //////// /************************************** Date: January 17, 2022 Function: Pub the topic whether the robot finds the infrared signal (charging station) 功能: 发布机器人是否寻找到红外信号(充电桩)的话题 ***************************************/ void turn_on_robot::Publish_RED() { std_msgs::msg::UInt8 msg; msg.data=Red; RED_publisher->publish(msg); } /************************************** Date: January 14, 2022 Function: Publish a topic about whether the robot is charging 功能: 发布机器人是否在充电的话题 ***************************************/ void turn_on_robot::Publish_Charging() { static bool last_charging; std_msgs::msg::Bool msg; msg.data=Charging; Charging_publisher->publish(msg); if(last_charging==false && Charging==true)cout<<GREEN<<"Robot is charging."<<endl<<RESET; if(last_charging==true && Charging==false)cout<<RED <<"Robot charging has disconnected."<<endl<<RESET; last_charging=Charging; } /************************************** Date: January 28, 2021 Function: Publish charging current information 功能: 发布充电电流信息 ***************************************/ void turn_on_robot::Publish_ChargingCurrent() { std_msgs::msg::Float32 msg; msg.data=Charging_Current; Charging_current_publisher->publish(msg); } /************************************** Date: March 1, 2022 Function: Infrared connection speed topic subscription Callback function, according to the subscription command through the serial port to set the infrared connection speed 功能: 红外对接速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令设置红外对接速度 ***************************************/ void turn_on_robot::Red_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //帧头0X7B Send_Data.tx[1] = 3; //Infrared docking speed setting flag bit = 3 //红外对接速度设置标志位=3 Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot's Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //BCC check //BCC校验 Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //帧尾0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 14, 2022 Function: Subscription robot recharge flag bit topic, used to tell the lower machine speed command is normal command or recharge command 功能: 订阅机器人是否回充标志位话题,用于告诉下位机速度命令是正常命令还是回充命令 ***************************************/ void turn_on_robot::Recharge_Flag_Callback(const std_msgs::msg::Int8::SharedPtr Recharge_Flag) { AutoRecharge=Recharge_Flag->data; } //服务 void turn_on_robot::Set_Charge_Callback(const shared_ptr<turtlesim::srv::Spawn::Request> req,shared_ptr<turtlesim::srv::Spawn::Response> res) { Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //֡ͷ0X7B if(round(req->x)==1) Send_Data.tx[1] = 1; else if(round(req->x)==2) Send_Data.tx[1] = 2; else if(round(req->x)==0) Send_Data.tx[1] = 0,AutoRecharge=0; Send_Data.tx[2] = 0; Send_Data.tx[3] = 0; Send_Data.tx[4] = 0; Send_Data.tx[5] = 0; Send_Data.tx[6] = 0; Send_Data.tx[7] = 0; Send_Data.tx[8] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCCУ��λ������μ�Check_Sum���� Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //֡β0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //ͨ����������λ���������� } catch (serial::IOException& e) { res->name = "false"; } if( Send_Data.tx[1]==0 ) { if(charge_set_state==0) AutoRecharge=0,res->name = "true"; else res->name = "false"; } else { if(charge_set_state==1) res->name = "true"; else res->name = "false"; } return; } ////////// 回充发布与回调 //////// /************************************** Date: January 28, 2021 Function: Serial port communication check function, packet n has a byte, the NTH -1 byte is the check bit, the NTH byte bit frame end.Bit XOR results from byte 1 to byte n-2 are compared with byte n-1, which is a BCC check Input parameter: Count_Number: Check the first few bytes of the packet 功能: 串口通讯校验函数,数据包n有个字节,第n-1个字节为校验位,第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比,即为BCC校验 输入参数: Count_Number:数据包前几个字节加入校验 mode:对发送数据还是接收数据进行校验 ***************************************/ unsigned char turn_on_robot::Check_Sum(unsigned char Count_Number,unsigned char mode) { unsigned char check_sum=0,k; if(mode==0) //Receive data mode //接收数据模式 { for(k=0;k<Count_Number;k++) { check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或 } } if(mode==1) //Send data mode //发送数据模式 { for(k=1;k<Count_Number;k++) { check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或 } } return check_sum; //Returns the bitwise XOR result //返回按位异或结果 } //自动回充专用校验位 unsigned char turn_on_robot::Check_Sum_AutoCharge(unsigned char Count_Number,unsigned char mode) { unsigned char check_sum=0,k; if(mode==0) //Receive data mode //接收数据模式 { for(k=0;k<Count_Number;k++) { check_sum=check_sum^Receive_AutoCharge_Data.rx[k]; //By bit or by bit //按位异或 } } return check_sum; } /************************************** Date: November 18, 2021 Function: Read and verify the data sent by the lower computer frame by frame through the serial port, and then convert the data into international units 功能: 通过串口读取并逐帧校验下位机发送过来的数据,然后数据转换为国际单位 ***************************************/ bool turn_on_robot::Get_Sensor_Data_New() { short transition_16=0; //Intermediate variable //中间变量 //uint8_t i=0; uint8_t check=0,check2=0, error=1,error2=1,Receive_Data_Pr[1]; //Temporary variable to save the data of the lower machine //临时变量,保存下位机数据 static int count,count2; //Static variable for counting //静态变量,用于计数 Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据 /*//View the received raw data directly and debug it for use//直接查看接收到的原始数据,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7], Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15], Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]); */ Receive_Data.rx[count] = Receive_Data_Pr[0]; //Fill the array with serial data //串口数据填入数组 Receive_AutoCharge_Data.rx[count2] = Receive_Data_Pr[0]; Receive_Data.Frame_Header = Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B Receive_Data.Frame_Tail = Receive_Data.rx[23]; //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D //接收到自动回充数据的帧头、上一个数据是24字节的帧尾,表明自动回充数据开始到来 if((Receive_Data_Pr[0] == AutoCharge_HEADER )||count2>0) count2++; else count2=0; if(Receive_Data_Pr[0] == FRAME_HEADER || count>0) //Ensure that the first data in the array is FRAME_HEADER //确保数组第一个数据为FRAME_HEADER count++; else count=0; //自动回充数据处理 if(count2 == AutoCharge_DATA_SIZE) { count2=0; if(Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-1]==AutoCharge_TAIL) //确认帧尾 { check2 = Check_Sum_AutoCharge(6,0);//校验位计算 if(check2 == Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-2]) //校验正确 { error2=0; } if(error2 == 0) //校验正确开始赋值 { transition_16 = 0; transition_16 |= Receive_AutoCharge_Data.rx[1]<<8; transition_16 |= Receive_AutoCharge_Data.rx[2]; Charging_Current = transition_16/1000+(transition_16 % 1000)*0.001; //充电电流 Red = Receive_AutoCharge_Data.rx[3]; //红外接受状态 Charging = Receive_AutoCharge_Data.rx[4];//小车充电状态 charge_set_state = Receive_AutoCharge_Data.rx[5]; check_AutoCharge_data = true; //数据成功接收标志位 } } } if(count == 24) //Verify the length of the packet //验证数据包的长度 { count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备 if(Receive_Data.Frame_Tail == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾 { check=Check_Sum(22,READ_DATA_CHECK); //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错 if(check == Receive_Data.rx[22]) { error=0; //XOR bit check successful //异或位校验成功 } if(error == 0) { /*//Check receive_data.rx for debugging use //查看Receive_Data.rx,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7], Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15], Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */ Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位 Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度 //Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis //获取运动底盘Y方向速度,Y速度仅在全向移动机器人底盘有效 //Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度 //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250 //Mpu6050仅代表IMU,不指代特定型号,既可以是MPU6050也可以是MPU9250 Mpu6050_Data.imu_rol = IMU_Trans(Receive_Data.rx[4],Receive_Data.rx[5]) * 0.01 * 3.14159/180; Mpu6050_Data.imu_yaw = IMU_Trans(Receive_Data.rx[6],Receive_Data.rx[7]) * 0.01 * 3.14159/180; Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); //Get the X-axis acceleration of the IMU //获取IMU的X轴加速度 Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU //获取IMU的Y轴加速度 Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU //获取IMU的Z轴加速度 Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度 Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度 Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度 //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2 //线性加速度单位转化,和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2 Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data * 0.01 * 9.8; Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data * 0.01 * 9.8; Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data * 0.01 * 9.8; //The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s //Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy //陀螺仪单位转化,和STM32的IMU初始化的时候的量程有关,这里IMU的陀螺仪的量程是±500°/s //因为机器人一般Z轴速度不快,降低量程可以提高精度 Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * 0.01 *3.14159/180; Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * 0.01 *3.14159/180; Mpu6050.angular_velocity.z = Mpu6050_Data.gyros_z_data * 0.01 *3.14159/180; //Get the battery voltage //获取电池电压 transition_16 = 0; transition_16 |= Receive_Data.rx[20]<<8; transition_16 |= Receive_Data.rx[21]; Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v) return true; } } } return false; } /************************************** Date: January 28, 2021 Function: Loop access to the lower computer data and issue topics 功能: 循环获取下位机数据与发布话题 ***************************************/ void turn_on_robot::Control() { //_Last_Time = ros::Time::now(); _Last_Time = rclcpp::Node::now(); while(rclcpp::ok()) { try { //_Now = ros::Time::now(); _Now = rclcpp::Node::now(); Sampling_Time = (_Now - _Last_Time).seconds(); //Retrieves time interval, which is used to integrate velocity to obtain displacement (mileage) //获取时间间隔,用于积分速度获得位移(里程) if (true == Get_Sensor_Data_New()) //The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units //通过串口读取并校验下位机发送过来的数据,然后数据转换为国际单位 { //Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m //Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time; //The angular displacement about the Z axis, in rad //绕Z轴的角位移,单位:rad Robot_Pos.X+=(Robot_Vel.X * cos(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m Robot_Pos.Y+=(Robot_Vel.X * sin(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z = 0 ; //Calculate the three-axis attitude from the IMU with the angular velocity around the three-axis and the three-axis acceleration //通过IMU绕三轴角速度与三轴加速度计算三轴姿态 Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\ Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z); Publish_Odom(); //Pub the speedometer topic //发布里程计话题 Publish_ImuSensor(); //Pub the IMU topic //发布IMU话题 Publish_Voltage(); //Pub the topic of power supply voltage //发布电源电压话题 _Last_Time = _Now; //Record the time and use it to calculate the time interval //记录时间,用于计算时间间隔 } //自动回充数据话题 if(check_AutoCharge_data) { Publish_Charging(); //Pub a topic about whether the robot is charging //发布机器人是否在充电的话题 Publish_RED(); //Pub the topic whether the robot finds the infrared signal (charging station) //发布机器人是否寻找到红外信号(充电桩)的话题 Publish_ChargingCurrent(); //Pub the charging current topic //发布充电电流话题 check_AutoCharge_data = false; } rclcpp::spin_some(this->get_node_base_interface()); //The loop waits for the callback function //循环等待回调函数 } catch (const rclcpp::exceptions::RCLError & e ) { RCLCPP_ERROR(this->get_logger(),"unexpectedly failed whith %s",e.what()); } } } /************************************** Date: January 28, 2021 Function: Constructor, executed only once, for initialization 功能: 构造函数, 只执行一次,用于初始化 ***************************************/ turn_on_robot::turn_on_robot():rclcpp::Node ("wheeltec_robot") { Sampling_Time=0; Power_voltage=0; //Clear the data //清空数据 memset(&Robot_Pos, 0, sizeof(Robot_Pos)); memset(&Robot_Vel, 0, sizeof(Robot_Vel)); memset(&Receive_Data, 0, sizeof(Receive_Data)); memset(&Send_Data, 0, sizeof(Send_Data)); memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data)); //ros::NodeHandle private_nh("~"); //Create a node handle //创建节点句柄 //The private_nh.param() entry parameter corresponds to the initial value of the name of the parameter variable on the parameter server //private_nh.param()入口参数分别对应:参数服务器上的名称 参数变量名 初始值 this->declare_parameter<int>("serial_baud_rate",115200); this->declare_parameter<std::string>("usart_port_name", "/dev/ttyACM0"); this->declare_parameter<std::string>("odom_frame_id", "odom_combined"); this->declare_parameter<std::string>("robot_frame_id", "base_footprint"); this->declare_parameter<std::string>("gyro_frame_id", "gyro_link"); this->get_parameter("serial_baud_rate", serial_baud_rate);//Communicate baud rate 115200 to the lower machine //和下位机通信波特率115200 this->get_parameter("usart_port_name", usart_port_name);//Fixed serial port number //固定串口号 this->get_parameter("odom_frame_id", odom_frame_id);//The odometer topic corresponds to the parent TF coordinate //里程计话题对应父TF坐标 this->get_parameter("robot_frame_id", robot_frame_id);//The odometer topic corresponds to sub-TF coordinates //里程计话题对应子TF坐标 this->get_parameter("gyro_frame_id", gyro_frame_id);//IMU topics correspond to TF coordinates //IMU话题对应TF坐标 odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 2);//Create the odometer topic publisher //创建里程计话题发布者 imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 2); //Create an IMU topic publisher //创建IMU话题发布者 voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);//Create a battery-voltage topic publisher //创建电池电压话题发布者 //回充发布者 Charging_publisher = create_publisher<std_msgs::msg::Bool>("robot_charging_flag", 10); Charging_current_publisher = create_publisher<std_msgs::msg::Float32>("robot_charging_current", 10); RED_publisher = create_publisher<std_msgs::msg::UInt8>("robot_red_flag", 10); //回充订阅者 Red_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "red_vel", 10, std::bind(&turn_on_robot::Red_Vel_Callback, this, std::placeholders::_1)); Recharge_Flag_Sub = create_subscription<std_msgs::msg::Int8>( "robot_recharge_flag", 10, std::bind(&turn_on_robot::Recharge_Flag_Callback, this,std::placeholders::_1)); //回充服务提供 SetCharge_Service=this->create_service<turtlesim::srv::Spawn>\ ("/set_charge",std::bind(&turn_on_robot::Set_Charge_Callback,this, std::placeholders::_1 ,std::placeholders::_2)); //Set the velocity control command callback function //速度控制命令订阅回调函数设置 Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "cmd_vel", 2, std::bind(&turn_on_robot::Cmd_Vel_Callback, this, _1)); RCLCPP_INFO(this->get_logger(),"wheeltec_robot Data ready"); //Prompt message //提示信息 try { //Attempts to initialize and open the serial port //尝试初始化与开启串口 Stm32_Serial.setPort(usart_port_name); //Select the serial port number to enable //选择要开启的串口号 Stm32_Serial.setBaudrate(serial_baud_rate); //Set the baud rate //设置波特率 serial::Timeout _time = serial::Timeout::simpleTimeout(2000); //Timeout //超时等待 Stm32_Serial.setTimeout(_time); Stm32_Serial.open(); //Open the serial port //开启串口 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),"wheeltec_robot can not open serial port,Please check the serial port cable! "); //If opening the serial port fails, an error message is printed //如果开启串口失败,打印错误信息 } if(Stm32_Serial.isOpen()) { RCLCPP_INFO(this->get_logger(),"wheeltec_robot serial port opened"); //Serial port opened successfully //串口开启成功提示 } } /************************************** Date: January 28, 2021 Function: Destructor, executed only once and called by the system when an object ends its life cycle 功能: 析构函数,只执行一次,当对象结束其生命周期时系统会调用这个函数 ***************************************/ turn_on_robot::~turn_on_robot() { //Sends the stop motion command to the lower machine before the turn_on_robot object ends //对象turn_on_robot结束前向下位机发送停止运动命令 Send_Data.tx[0]=0x06; Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //The target velocity of the X-axis of the robot //机器人X轴的目标线速度 Send_Data.tx[4] = 0; Send_Data.tx[3] = 0; //The target velocity of the Y-axis of the robot //机器人Y轴的目标线速度 Send_Data.tx[6] = 0; Send_Data.tx[5] = 0; //The target velocity of the Z-axis of the robot //机器人Z轴的目标角速度 Send_Data.tx[8] = 0; Send_Data.tx[7] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //Check the bits for the Check_Sum function //校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Send data to the serial port //向串口发数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),"Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } Stm32_Serial.close(); //Close the serial port //关闭串口 RCLCPP_INFO(this->get_logger(),"Shutting down"); //Prompt message //提示信息 } turn_on_wheeltec_robot.launch.py文件代码为: import os from pathlib import Path import launch from launch.actions import SetEnvironmentVariable from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import PushRosNamespace import launch_ros.actions from launch.conditions import IfCondition from launch.conditions import UnlessCondition def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('turn_on_wheeltec_robot') launch_dir = os.path.join(bringup_dir, 'launch') ekf_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'ekf.yaml') ekf_carto_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'ekf_carto.yaml') imu_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'imu.yaml') carto_slam = LaunchConfiguration('carto_slam', default='false') carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false') wheeltec_robot = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')), ) robot_ekf = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wheeltec_ekf.launch.py')), launch_arguments={'carto_slam':carto_slam}.items(), ) base_to_link = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name='base_to_link', arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'], ) base_to_gyro = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name='base_to_gyro', arguments=['0', '0', '0','0', '0','0','base_link','gyro_link'], ) imu_filter_node = launch_ros.actions.Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', parameters=[imu_config] ) # joint_state_publisher_node = launch_ros.actions.Node( # package='joint_state_publisher', # executable='joint_state_publisher', # name='joint_state_publisher', # ) #select a robot model,the default model is mini_mec #minibot.launch.py contains commonly used robot models #launch_arguments choices:mini_mec/mini_akm/mini_tank/mini_4wd/mini_diff/mini_omni/brushless_senior_diff #!!!At the same time, you need to modify ld.add_action(minibot_type) and #ld.add_action(flagship_type) minibot_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description_minibot.launch.py')), launch_arguments={'bike_robot': 'true'}.items(), ) #robot_mode_description.launch.py contains flagship products, usually larger chassis robots #launch_arguments choices: #senior_akm/top_akm_bs/top_akm_dl #senior_mec_bs/senior_mec_dl/top_mec_bs/top_mec_dl/ mec_EightDrive_robot/flagship_mec_bs_robot/flagship_mec_dl_robot�� #senior_omni/top_omni #senior_4wd_bs_robot/senior_4wd_dl_robot/flagship_4wd_bs_robot/flagship_4wd_dl_robot/top_4wd_bs_robot/top_4wd_dl_robot #senior_diff_robot/four_wheel_diff_bs/four_wheel_diff_dl/flagship_four_wheel_diff_bs_robot/flagship_four_wheel_diff_dl_robot #!!!At the same time, you need to modify ld.add_action(flagship_type) and #ld.add_action(minibot_type) flagship_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')), launch_arguments={'senior_akm': 'true'}.items(), ) ld = LaunchDescription() ld.add_action(minibot_type) #ld.add_action(flagship_type) ld.add_action(carto_slam_dec) ld.add_action(wheeltec_robot) ld.add_action(base_to_link) ld.add_action(base_to_gyro) # ld.add_action(joint_state_publisher_node) ld.add_action(imu_filter_node) ld.add_action(robot_ekf) return ld base_serial.launch.py文件代码为: from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition import launch_ros.actions #def launch(launch_descriptor, argv): def generate_launch_description(): return LaunchDescription([ launch_ros.actions.Node( package='turn_on_wheeltec_robot', executable='wheeltec_robot_node', output='screen', parameters=[{'usart_port_name': '/dev/ttyACM0', 'serial_baud_rate':115200, 'robot_frame_id': 'base_footprint', 'odom_frame_id': 'odom_combined', 'cmd_vel': 'cmd_vel',}], ) ]) robot_mode_description_minibot.launch.py文件代码为: import os from ament_index_python.packages import get_package_share_directory import launch_ros.actions from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml def generate_robot_node(robot_urdf,child): return launch_ros.actions.Node( package='robot_state_publisher', executable='robot_state_publisher', name=f'robot_state_publisher_{child}', arguments=[os.path.join(get_package_share_directory('wheeltec_robot_urdf'), 'urdf', robot_urdf)], ) def generate_static_transform_publisher_node(translation, rotation, parent, child): return launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name=f'base_to_{child}', arguments=[translation[0], translation[1], translation[2], rotation[0], rotation[1], rotation[2], parent, child], ) def generate_launch_description(): bike_robot = LaunchConfiguration('bike_robot', default='false') bike_robot_ = GroupAction( condition=IfCondition(bike_robot), actions=[ generate_robot_node('xuan_bike_robot.urdf','bike_robot'), generate_static_transform_publisher_node(['0.048', '0', '0.18'], ['0', '0', '0'], 'base_link', 'laser'), generate_static_transform_publisher_node(['0.048', '0', '0.18'], ['0', '0', '0'], 'base_link', 'camera_link'), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables # Declare the launch options #ld.add_action(declare_use_composition_cmd) # Add the actions to launch all of the localiztion nodes ld.add_action(mini_mec_) ld.add_action(mini_akm_) ld.add_action(mini_tank_) ld.add_action(mini_4wd_) ld.add_action(mini_diff_) ld.add_action(brushless_senior_diff_) ld.add_action(bike_robot_) return ld xuan_bike_robot.urdf文件代码为: <?xml version="1.0"?> <robot name="wheeltec_robot"> <link name="base_link"> <visual> <geometry> <mesh filename="file:///home/deepbot/ros-relate/wheeltec_ros2/src/wheeltec_robot_urdf/wheeltec_robot_urdf/meshes/Xuan-release.stl"scale="0.001 0.001 0.001"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="blue"> <color rgba="0 0 0.8 0.5"/> </material> </visual> </link> </robot>
05-14
make O=build/ac5x_pro >>> host-skeleton Executing pre-build script board/tplink/common/pre_build.sh /project/fep_switch/tplink/buildroot/build/ac5x_pro/build/tplink /project/fep_switch/tplink/buildroot/build/ac5x_pro/build/tplink/archive /project/fep_switch/tplink/buildroot/build/ac5x_pro/target/../archive >>> fep 1.0 Downloading wget --passive-ftp -nd -t 3 -O '/project/fep_switch/tplink/buildroot/build/ac5x_pro/build/.fep_armv8_1.0.tar.gz.v1zzHV/output' 'http://jenkins1.ep.tp-link.com/job/FEP_SOURCE_20240927/lastSuccessfulBuild/artifact/image/fep_armv8_1.0.tar.gz' --2025-08-26 14:00:06-- http://jenkins1.ep.tp-link.com/job/FEP_SOURCE_20240927/lastSuccessfulBuild/artifact/image/fep_armv8_1.0.tar.gz 正在解析主机 jenkins1.ep.tp-link.com (jenkins1.ep.tp-link.com)... 10.176.71.29 正在连接 jenkins1.ep.tp-link.com (jenkins1.ep.tp-link.com)|10.176.71.29|:80... 已连接。 已发出 HTTP 请求,正在等待回应... 404 Not Found 2025-08-26 14:00:07 错误 404:Not Found。 wget --passive-ftp -nd -t 3 -O '/project/fep_switch/tplink/buildroot/build/ac5x_pro/build/.fep_armv8_1.0.tar.gz.fl9Ywa/output' 'http://sources.buildroot.net/fep/fep_armv8_1.0.tar.gz' --2025-08-26 14:00:07-- http://sources.buildroot.net/fep/fep_armv8_1.0.tar.gz 正在解析主机 sources.buildroot.net (sources.buildroot.net)... 104.26.0.37, 172.67.72.56, 104.26.1.37, ... 正在连接 sources.buildroot.net (sources.buildroot.net)|104.26.0.37|:80... 已连接。 已发出 HTTP 请求,正在等待回应... 404 Not Found 2025-08-26 14:00:07 错误 404:Not Found。 wget --passive-ftp -nd -t 3 -O '/project/fep_switch/tplink/buildroot/build/ac5x_pro/build/.fep_armv8_1.0.tar.gz.irQQqs/output' 'http://sources.buildroot.net/fep_armv8_1.0.tar.gz' --2025-08-26 14:00:07-- http://sources.buildroot.net/fep_armv8_1.0.tar.gz 正在解析主机 sources.buildroot.net (sources.buildroot.net)... 104.26.0.37, 104.26.1.37, 172.67.72.56, ... 正在连接 sources.buildroot.net (sources.buildroot.net)|104.26.0.37|:80... 已连接。 已发出 HTTP 请求,正在等待回应... 404 Not Found 2025-08-26 14:00:07 错误 404:Not Found。 package/tplink/tplink-generic.mk:185: recipe for target '/project/fep_switch/tplink/buildroot/build/ac5x_pro/build/tplink/fep-1.0/.stamp_downloaded' failed make[1]: *** [/project/fep_switch/tplink/buildroot/build/ac5x_pro/build/tplink/fep-1.0/.stamp_downloaded] Error 1 Makefile:84: recipe for target '_all' failed make: *** [_all] Error 2分析下这里错误的原因是什么
08-27
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值