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

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 initializedconst 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.");}
// 计算当前时间与最新收到位置信息的时间戳之间的差值elapsedconst 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_fitterif (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_namereq->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

…详情请参照古月居