Autoware 定位之初始姿态输入（九）

news2024/9/20 8:01:28

0. 简介

这一讲按照《Autoware 技术代码解读（三）》梳理的顺序，我们来说一说Autoware中的初始化操作，这个软件包当中完成了ekf_localizer发送初始姿态的包。它接收来自GNSS/用户的粗略估计的初始姿态。将姿态传递给ndt_scan_matcher，并通过服务从ndt_scan_matcher获取计算出的自我姿态。最后，它将初始姿态发布到ekf_localizer。

在这里插入图片描述

1. 代码阅读

1.1 copy_vector_to_array.hpp

定义了两个模板函数。第一个函数copy_vector_to_array将std::vector中的数据复制到std::array中。它首先检查输入的vector和目标数组array的大小是否相同，如果不同则抛出异常。然后使用std::copy_n函数将vector中的前N个元素复制到array中。

第二个函数get_covariance_parameter的作用是从节点获取具有特定名称的协方差参数，并将其转换为长度为36的std::array。它首先从节点中获取参数，然后创建一个长度为36的std::array并调用copy_vector_to_array函数将获取的参数复制到std::array中，最后返回这个std::array。

/// @brief 将std::vector中的数据复制到std::array中
/// @tparam T 向量和数组中元素的类型
/// @tparam N 表示目标数组的大小
/// @param vector 输入的std::vector对象
/// @param array 要将数据复制到的std::array对象
template <typename T, size_t N>
void copy_vector_to_array(const std::vector<T> & vector, std::array<T, N> & array)
{
  if (N != vector.size()) {//检查输入的vector和目标数组array的大小是否相同
    // throws the error to prevent causing an anonymous bug
    // such as only partial array is initialized
    const auto v = std::to_string(vector.size());
    const auto n = std::to_string(N);
    throw std::invalid_argument(
      "Vector size (which is " + v + ") is different from the copy size (which is " + n + ")");//如果不同，则抛出一个std::invalid_argument异常
  }
  std::copy_n(vector.begin(), N, array.begin());//它使用std::copy_n函数将vector中的前N个元素复制到array中
}

/// @brief 该函数的作用是从节点获取具有特定名称的协方差参数，并将其转换为长度为36的std::array。
/// @tparam NodeT 表示节点的类型
/// @param node 指向节点的指针
/// @param name 要获取的参数的名称
/// @return 
template <class NodeT>
std::array<double, 36> get_covariance_parameter(NodeT * node, const std::string & name)
{
  const auto parameter = node->template declare_parameter<std::vector<double>>(name);
  std::array<double, 36> covariance;
  copy_vector_to_array(parameter, covariance);
  return covariance;
}

1.2 ekf_localization_trigger_module.cpp

这段代码是一个用于与 ROS 节点通信的模块，它初始化了一个日志记录器和一个用于与 EKF 触发服务通信的客户端。构造函数接受一个指向 ROS 节点的指针，然后初始化了日志记录器和客户端。

send_request 函数用于发送 EKF 触发请求，接受一个布尔类型的参数，表示是否激活或停止 EKF 定位模块。根据输入的参数，确定了具体的 EKF 触发命令名称，然后检查服务是否准备就绪，如果不准备就绪则抛出异常。

接着异步发送 EKF 触发请求，然后通过获取响应结果来输出相应的信息，表示 EKF 触发成功或失败。整体来说，这段代码是一个用于与 EKF 触发服务通信的模块，提供了初始化和发送请求的功能。通过构造函数初始化客户端和日志记录器，然后通过 send_request 函数发送 EKF 触发请求，并处理响应结果。

/// @brief 构造函数，它接受一个指向ROS节点的指针，并在其中初始化了一个日志记录器和一个用于与EKF触发服务通信的客户端
/// @param node 指向ROS节点的指针
EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node)
: logger_(node->get_logger())
{
  client_ekf_trigger_ = node->create_client<SetBool>("ekf_trigger_node");
}

/// @brief 用于发送EKF触发请求。它接受一个布尔类型的参数flag
/// @param flag 是否激活或者停止EKF定位模块
void EkfLocalizationTriggerModule::send_request(bool flag) const
{
  const auto req = std::make_shared<SetBool::Request>();
  std::string command_name;
  req->data = flag;
  if (flag) {
    command_name = "Activation";
  } else {
    command_name = "Deactivation";
  }//根据输入的flag值，确定了EKF触发的具体命令名称，分别为"Activation"和"Deactivation"。

//检查EKF触发服务是否准备就绪
  if (!client_ekf_trigger_->service_is_ready()) {
    throw component_interface_utils::ServiceUnready("EKF triggering service is not ready");
  }

    //异步发送EKF触发请求
  auto future_ekf = client_ekf_trigger_->async_send_request(req);

    //通过future_ekf获取响应结果，在成功时输出信息表示EKF触发成功
  if (future_ekf.get()->success) {
    RCLCPP_INFO(logger_, "EKF %s succeeded", command_name.c_str());
  } else {
    RCLCPP_INFO(logger_, "EKF %s failed", command_name.c_str());
    throw ServiceException(
      Initialize::Service::Response::ERROR_ESTIMATION, "EKF " + command_name + " failed");
  }
}

1.3 gnss_module.cpp

这段代码是一个用于处理GNSS定位模块数据的类，其中包括了构造函数和获取位置信息的函数。构造函数接受一个指向ROS节点的指针作为参数，用于初始化订阅GNSS定位数据的功能。获取位置信息的函数首先检查是否收到了GNSS定位数据，然后计算位置信息的时间戳是否超时。如果位置信息仍在有效期内，就获取并处理最新的位置信息，并对位置信息进行拟合处理，得到拟合结果。最终返回处理后的位置信息。整体来说，这段代码主要是用于从GNSS定位模块获取位置信息，并对位置信息进行处理和拟合。

/// @brief GnssModule类的构造函数，接受一个rclcpp::Node类型的指针作为参
/// @param node 指向ROS节点的指针
GnssModule::GnssModule(rclcpp::Node * node) : fitter_(node)
{
  sub_gnss_pose_ = node->create_subscription<PoseWithCovarianceStamped>(
    "gnss_pose_cov", 1, [this](PoseWithCovarianceStamped::ConstSharedPtr msg) { pose_ = msg; });

  clock_ = node->get_clock();
  timeout_ = node->declare_parameter<double>("gnss_pose_timeout");
}

/// @brief 该函数用于获取GNSS定位模块的位置信息
/// @return 返回一个PoseWithCovarianceStamped类型的对象
geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose()
{
  using Initialize = localization_interface::Initialize;
// 函数内部首先检查是否收到了GNSS定位数据
  if (!pose_) {
    throw component_interface_utils::ServiceException(
      Initialize::Service::Response::ERROR_GNSS, "The GNSS pose has not arrived.");
  }
// 计算当前时间与最新收到位置信息的时间戳之间的差值elapsed
  const auto elapsed = rclcpp::Time(pose_->header.stamp) - clock_->now();
  if (timeout_ < elapsed.seconds()) {
    throw component_interface_utils::ServiceException(
      Initialize::Service::Response::ERROR_GNSS, "The GNSS pose is out of date.");
  }
    //如果位置信息仍在有效期内，那么就获取并处理最新的位置信息
  PoseWithCovarianceStamped pose = *pose_;
  const auto fitted = fitter_.fit(pose.pose.pose.position, pose.header.frame_id);//位置信息进行拟合处理，得到拟合结果fitted，对应了map下面的map_height_fitter
  if (fitted) {
    pose.pose.pose.position = fitted.value();
  }
  return pose;
}

1.4 ndt_localization_trigger_module.cpp

用于与 NDT（Normal Distribution Transform）触发器服务进行通信。构造函数初始化了该类的日志记录器和与 NDT 触发器服务的通信客户端。send_request 方法用于向 NDT 触发器服务发送请求，请求包括一个布尔值，表示要执行的操作。根据布尔值的不同，确定要执行的命令名称，然后通过客户端异步发送请求，并跟踪请求的状态。如果请求成功，则记录日志并返回成功消息；如果失败，则记录日志并抛出服务异常。在发送请求之前，还会检查 NDT 触发器服务是否准备就绪，如果不准备就绪，则抛出服务未准备就绪的异常。

/// @brief 构造函数，用于初始化NdtLocalizationTriggerModule对象
/// @param node 指向ROS节点的指针
NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node)
: logger_(node->get_logger())
{
  client_ndt_trigger_ = node->create_client<SetBool>("ndt_trigger_node");
}

/// @brief 发送请求给Ndt触发器服务
/// @param flag bool类型的flag，表示要执行的操作
void NdtLocalizationTriggerModule::send_request(bool flag) const
{
  const auto req = std::make_shared<SetBool::Request>();
  std::string command_name;//根据flag的值确定command_name
  req->data = flag;
  if (flag) {
    command_name = "Activation";
  } else {
    command_name = "Deactivation";
  }

  if (!client_ndt_trigger_->service_is_ready()) {// 检查ndt触发器服务是否准备就绪
    throw component_interface_utils::ServiceUnready("NDT triggering service is not ready");
  }

  auto future_ndt = client_ndt_trigger_->async_send_request(req);//通过客户端client_ndt_trigger_异步发送请求req，并获得future_ndt以跟踪请求的状态

  if (future_ndt.get()->success) {
    RCLCPP_INFO(logger_, "NDT %s succeeded", command_name.c_str());
  } else {
    RCLCPP_INFO(logger_, "NDT %s failed", command_name.c_str());
    throw ServiceException(
      Initialize::Service::Response::ERROR_ESTIMATION, "NDT " + command_name + " failed");
  }
}

1.5 stop_check_module.cpp

构造函数接受一个Node句柄和一个缓冲区时间作为参数，并使用这些参数初始化StopCheckModule对象。在构造函数中，它创建了一个订阅者(subscription)，用于订阅名为"stop_check_twist"的TwistWithCovarianceStamped消息，并指定回调函数为on_twist

…详情请参照古月居

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/2113268.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

Linux 删除虚拟环境命令

Linux 删除虚拟环境命令

查看当前都有哪些虚拟环境 # conda info --env 删除虚拟环境 py311 conda remove -n py311 --all
阅读更多...
【Canvas与钟表】干支表盘

【Canvas与钟表】干支表盘

【成图】 【代码】 <!DOCTYPE html> <html lang"utf-8"> <meta http-equiv"Content-Type" content"text/html; charsetutf-8"/> <head><title>387.干支表盘</title><style type"text/css">…
阅读更多...
热门宠物空气净化器测评，希喂、小米空气净化器真实测试对比

热门宠物空气净化器测评，希喂、小米空气净化器真实测试对比

随着宠物在家庭中占据越来越重要的地位&#xff0c;如何保持家中空气的清新成为了许多铲屎官关注的重点。市面上的宠物空气净化器琳琅满目&#xff0c;其中希喂和小米两款产品备受关注。今天我们就从外观设计、功能性、滤芯效果、噪音控制和性价比五个方面&#xff0c;来为大家…
阅读更多...
2024 9月最新PyCharm下载安装教程(详细步骤)附激活码！

2024 9月最新PyCharm下载安装教程(详细步骤)附激活码！

PyCharm安装教程 一、软件简介 PyCharm是一款Python IDE&#xff0c;其带有一整套可以帮助用户在使用Python语言开发时提高其效率的工具&#xff0c;比如&#xff0c; 调试、语法高亮、Project管理、代码跳转、智能提示、自动完成、单元测试、版本控制等等。此外&#xff0c;…
阅读更多...
设计模式-行为型模式-迭代器模式

设计模式-行为型模式-迭代器模式

1.迭代器模式的定义 迭代器模式提供一种对容器对象中的各个元素进行访问的方法&#xff0c;而不需要暴露该对象的内部细节&#xff1b; 在软件系统中&#xff0c;容器对象有两个职责&#xff1a;一是存储数据&#xff0c;二是遍历数据&#xff1b;从依赖性上看&#xff0c;前者…
阅读更多...
C语言代码练习（第十六天）

C语言代码练习（第十六天）

今日练习&#xff1a; 40、编写程序&#xff0c;用 getchar 函数读入两个字符给c1和c2&#xff0c;然后分别用 putchar 函数和 printf 函数输出这两个字符。 41、输入4个整数&#xff0c;要求按由小到大的顺序输出。 42、有4个圆塔&#xff0c;圆心分别为&#xff08;2.2)、(-2…
阅读更多...
笔记 13 ： 彭老师课本第 8 章， UART ，概念，帧格式 ， 工作原理，模块介绍，查看原理图 与 datasheet ，GPIO 组态 ，寄存器介绍

笔记 13 ： 彭老师课本第 8 章， UART ，概念，帧格式 ， 工作原理，模块介绍，查看原理图 与 datasheet ，GPIO 组态 ，寄存器介绍

&#xff08;94&#xff09; 开始学习通信。通信谢意要考虑时钟同步&#xff0c;是否双工通信&#xff0c;并行或串行通信等等&#xff1a; 低速协议用 uart &#xff0c; iic &#xff0c; spi &#xff0c; 高速协议用 pci 。can 总线支持远距离传输&#xff0c;如门禁&a…
阅读更多...
Cursor 使用 One API 配置 Anthropic Claude BaseURL 代理指南

Cursor 使用 One API 配置 Anthropic Claude BaseURL 代理指南

背景 Cursor IDE 原生只支持配置 ChatGPT 的 API Base URL,无法直接使用 Anthropic Claude 的 API。 本指南将介绍如何通过One API来解决这个问题,实现在Cursor中使用Claude API。 前置条件 部署One API https://github.com/songquanpeng/one-api 获取Anthropic Claude A…
阅读更多...
做运营，发布时间很重要

做运营，发布时间很重要

声明&#xff1a;此篇为 ai123.cn 原创文章&#xff0c;转载请标明出处链接&#xff1a;https://ai123.cn/#1 作为社交网络与媒体行业的内容运营&#xff0c;我常常被以下问题困扰&#xff1a;用户活跃时间难以预测、内容策划时间紧张、跨平台管理复杂、数据分析繁琐、创意枯竭…
阅读更多...
力扣 797. 所有可能路径【DFS】

力扣 797. 所有可能路径【DFS】

1. 题目 2. 代码 DFS &#xff0c; 直接见代码 class Solution { public:vector<int> path;vector<vector<int>> res; // 结果集void dfs(vector<vector<int>>& graph, int cur, int n){// 找出所有从节点 0 到节点 n-1 的路径// 下标从 …
阅读更多...
深入解析 Dubbo 的 attachments 机制及其应用场景

深入解析 Dubbo 的 attachments 机制及其应用场景

背景 在分布式系统中&#xff0c;服务之间的调用&#xff08;RPC调用&#xff09;是非常常见的。而在这种服务调用过程中&#xff0c;常常需要在不同服务之间传递一些上下文信息&#xff0c;比如用户身份信息、请求追踪ID、客户端IP等。Dubbo 提供的 attachments 机制&#xf…
阅读更多...
SysML图例-洗衣机

SysML图例-洗衣机

DDD领域驱动设计批评文集>> 《软件方法》强化自测题集>> 《软件方法》各章合集>> 对于许多学习SysML和MBSE的同学来说&#xff0c;比较头痛的问题之一是&#xff1a; 各种各样的教程里给出的案例&#xff0c;图都是画好了的&#xff01;如何从零开始用建模…
阅读更多...
LTspice模拟CCM和DCM模式的BUCK电路实验及参数计算

LTspice模拟CCM和DCM模式的BUCK电路实验及参数计算

关于BUCK电路的原理可以参考硬件工程师炼成之路写的《 手撕Buck&#xff01;Buck公式推导过程》.实验内容是将12V~5V的Buck电路仿真,要求纹波电压小于15mv. CCM和DCM的区别: CCM:在一个开关周期内&#xff0c;电感电流从不会到0. DCM:在开关周期内&#xff0c;电感电流总会到0.…
阅读更多...
缓存类型以及读写策略

缓存类型以及读写策略

缓存&#xff08;Cache&#xff09;是一种高效的数据存储技术&#xff0c;旨在提高数据访问速度。 它将频繁访问或最近使用的数据临时存储在更快速但较小的存储介质&#xff08;如内存&#xff09;中&#xff0c;以减少从较慢的存储设备&#xff08;如硬盘或远程服务器&#x…
阅读更多...
聊一聊大型网站稳定性建设思路

聊一聊大型网站稳定性建设思路

目录 架构阶段的稳定性建设项目 编码阶段的稳定性建设 测试阶段的稳定性建设 发布阶段的稳定性建设 运行阶段的稳定性建设项目 故障发生时的稳定性建设 网站稳定性的建设是一项综合的系统工程&#xff0c;就像人的健康一样&#xff0c;如果平时不注意健康饮食、不注意锻炼…
阅读更多...
浙大数据结构：02-线性结构4 Pop Sequence

浙大数据结构：02-线性结构4 Pop Sequence

这道题我们采用数组来模拟堆栈和队列。 简单说一下大致思路&#xff0c;我们用栈来存1234.....&#xff0c;队列来存输入的一组数据&#xff0c;栈与队列进行匹配&#xff0c;相同就pop 机翻 1、条件准备 stk是栈&#xff0c;que是队列。 tt指向的是栈中下标&#xff0c;fr…
阅读更多...
C++入门(05-2)从命令行执行C++编译器_GCC

C++入门(05-2)从命令行执行C++编译器_GCC

文章目录 GCC编译器1. 下载MinGW-w64&#xff0c;安装&#xff08;不推荐&#xff09;2. 使用MSYS2安装MinGW-w64&#xff08;推荐&#xff09;2.1 安装MSYS22.2 初始化和更新2.3 安装MinGW-w64编译器2.3 在MSYS2 Shell中导航到代码目录2.4 使用 g 编译2.5 运行可执行文件 GCC编…
阅读更多...
【Qt】qt发布Release版本,打包.exe可执行文件

【Qt】qt发布Release版本,打包.exe可执行文件

前言&#xff1a;Qt编译的可执行程序&#xff0c;如果直接运行&#xff0c;会出现0xc000007b报错&#xff0c;或者“由于占不到Qt5Network.dll,无法继续执行代码。重新安装程序可能会解决此问题”的报错&#xff0c;因为缺少相关的依赖包和动态库。 1、第一步&#xff1a;找到…
阅读更多...
仕考网：大三能考公务员吗?

仕考网：大三能考公务员吗?

本科生在大三阶段不具备报考资格&#xff0c;因为尚未完成学业并不是应届生。专科生在大三时则属于应届生&#xff0c;有资格参加公务员考试。 公务员报考条件包括&#xff1a; 1.国籍; 2.年龄于18至35周岁之间&#xff0c;对于当年毕业的硕士或博士研究生&#xff0c;年龄限…
阅读更多...
Python: #!/usr/bin/python3 #!/usr/bin/env python3

Python: #!/usr/bin/python3 #!/usr/bin/env python3

只能放在第一行&#xff0c;第二行就没有效果了。 1. 路径不同 #!/usr/bin/python3&& #!/usr/bin/env python3写在脚本语言第一行的目的是 想要以什么可执行程序去运行这个文件中的代码。 #!/usr/bin/python3是告诉操作系统执行这个脚本的时候&#xff0c;调用/usr/bin…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023