网站架构设计师有哪些学校可以报考,专业网站构建,国美网上商城,苏州seo公司排名0. 简介
这一讲按照《Autoware 技术代码解读#xff08;三#xff09;》梳理的顺序#xff0c;我们来说一说Autoware中的初始化操作#xff0c;这个软件包当中完成了ekf_localizer发送初始姿态的包。它接收来自GNSS/用户的粗略估计的初始姿态。将姿态传递给ndt_scan_match…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::vectorT vector, std::arrayT, 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::arraydouble, 36 get_covariance_parameter(NodeT * node, const std::string name)
{const auto parameter node-template declare_parameterstd::vectordouble(name);std::arraydouble, 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_clientSetBool(ekf_trigger_node);
}/// brief 用于发送EKF触发请求。它接受一个布尔类型的参数flag
/// param flag 是否激活或者停止EKF定位模块
void EkfLocalizationTriggerModule::send_request(bool flag) const
{const auto req std::make_sharedSetBool::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_subscriptionPoseWithCovarianceStamped(gnss_pose_cov, 1, [this](PoseWithCovarianceStamped::ConstSharedPtr msg) { pose_ msg; });clock_ node-get_clock();timeout_ node-declare_parameterdouble(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
用于与 NDTNormal 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_clientSetBool(ndt_trigger_node);
}/// brief 发送请求给Ndt触发器服务
/// param flag bool类型的flag表示要执行的操作
void NdtLocalizationTriggerModule::send_request(bool flag) const
{const auto req std::make_sharedSetBool::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
…详情请参照古月居
