Autoware 定位之ndt定位（八）

2024-09-08 03:28
文章标签 定位 autoware ndt

本文主要是介绍Autoware 定位之ndt定位（八），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

0. 简介

这一讲按照《Autoware 技术代码解读（三）》梳理的顺序，我们来说一说Autoware中的ndt定位，这个软件包有两个主要功能：1. 通过扫描匹配进行位置估计，2. 通过ROS服务使用蒙特卡洛方法估计初始位置
在这里插入图片描述

1. 代码阅读

1.1 debug.cpp

这段代码是一个用于生成用于调试的可视化标记数组的函数。它接受时间戳、坐标系、缩放比例、粒子信息和索引作为输入，并返回一个可视化标记数组。在函数内部，它首先创建了一个空的标记数组，然后设置了一个标记的各种属性，如时间戳、坐标系、类型、缩放、ID 和生命周期。接下来，它为粒子的初始姿态和结果姿态分别创建了三种不同的颜色标记，并将它们添加到标记数组中。最后，函数返回了生成的标记数组。

/// @brief 根据粒子信息生成用于调试的可视化标记数组
/// @param stamp 标记的时间戳
/// @param map_frame_ 标记的坐标系
/// @param scale 标记的缩放比例
/// @param particle 包含粒子姿态、分数、迭代次数等信息的结构体
/// @param i 粒子的索引
/// @return 
visualization_msgs::msg::MarkerArray make_debug_markers(const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_,const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i)
{// TODO(Tier IV): getNumSubscribers// TODO(Tier IV): clear old objectvisualization_msgs::msg::MarkerArray marker_array;//生成标记数组visualization_msgs::msg::Marker marker;marker.header.stamp = stamp;marker.header.frame_id = map_frame_;marker.type = visualization_msgs::msg::Marker::ARROW;marker.action = visualization_msgs::msg::Marker::ADD;marker.scale = scale;marker.id = static_cast<int32_t>(i);marker.lifetime = rclcpp::Duration::from_seconds(10.0);  // 10.0 is the lifetime in seconds.//用于初始姿态的变换概率颜色标记marker.ns = "initial_pose_transform_probability_color_marker";marker.pose = particle.initial_pose;marker.color = exchange_color_crc(particle.score / 4.5);marker_array.markers.push_back(marker);//用于初始姿态的迭代次数颜色标记marker.ns = "initial_pose_iteration_color_marker";marker.pose = particle.initial_pose;marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0);marker_array.markers.push_back(marker);//用于初始姿态的索引颜色标记marker.ns = "initial_pose_index_color_marker";marker.pose = particle.initial_pose;marker.color = exchange_color_crc(static_cast<double>(i) / 100.0);marker_array.markers.push_back(marker);//用于结果姿态的变换概率颜色标记marker.ns = "result_pose_transform_probability_color_marker";marker.pose = particle.result_pose;marker.color = exchange_color_crc(particle.score / 4.5);marker_array.markers.push_back(marker);//用于结果姿态的迭代次数颜色标记marker.ns = "result_pose_iteration_color_marker";marker.pose = particle.result_pose;marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0);marker_array.markers.push_back(marker);//用于结果姿态的索引颜色标记marker.ns = "result_pose_index_color_marker";marker.pose = particle.result_pose;marker.color = exchange_color_crc(static_cast<double>(i) / 100.0);marker_array.markers.push_back(marker);return marker_array;
}

1.2 map_module.cpp

该代码是一个地图模块的构造函数和回调函数，用于处理接收到的地图点云消息。构造函数初始化了地图模块，并创建了一个订阅地图点云消息的订阅者。回调函数接收地图点云消息，并使用接收到的数据执行了一系列操作，包括创建新的NDT对象、设置参数、输入地图点云数据、执行NDT匹配算法，并将匹配结果存储到共享指针中。整个过程使用了互斥锁来保护共享指针的访问，以确保线程安全。通过这些操作，地图模块能够及时地处理接收到的地图点云消息，并更新NDT对象，以便后续的定位和导航算法使用最新的地图数据进行匹配和定位。

/// @brief 构造函数，初始化地图模块
/// @param node 节点指针
/// @param ndt_ptr_mutex 互斥锁指针，用于保护NDT指针
/// @param ndt_ptr 指向NDT对象的共享指针
/// @param map_callback_group 回调函数组
MapModule::MapModule(rclcpp::Node * node, std::mutex * ndt_ptr_mutex,std::shared_ptr<NormalDistributionsTransform> ndt_ptr,rclcpp::CallbackGroup::SharedPtr map_callback_group)
: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex)
{auto map_sub_opt = rclcpp::SubscriptionOptions();//创建订阅选项map_sub_opt.callback_group = map_callback_group;//设置回调函数组map_points_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>("pointcloud_map", rclcpp::QoS{1}.transient_local(),std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt);//订阅地图点云
}/// @brief 处理接收到的地图点云消息
/// @param map_points_msg_ptr 接收到的地图点云消息指针
void MapModule::callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
{//接收到的地图点云消息，创建一个新的NDT对象NormalDistributionsTransform new_ndt;new_ndt.setParams(ndt_ptr_->getParams());//设置参数并输入地图点云数据pcl::shared_ptr<pcl::PointCloud<PointTarget>> map_points_ptr(new pcl::PointCloud<PointTarget>);pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);new_ndt.setInputTarget(map_points_ptr);// create Thread// detach// 执行NDT匹配算法，得到匹配结果auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();new_ndt.align(*output_cloud);// swapndt_ptr_mutex_->lock();*ndt_ptr_ = new_ndt;ndt_ptr_mutex_->unlock();
}

1.3 map_update_module.cpp

这段代码是一个地图更新模块，用于在自动驾驶系统中更新地图数据。首先，它定义了一个模板函数norm_xy，用于计算两个点之间的欧氏距离。然后，它实现了一个MapUpdateModule类，用于初始化地图更新模块，并包括了处理EKF里程计数据的回调函数、地图更新定时器的回调函数以及更新地图数据的函数。在初始化时，它接受节点指针、互斥锁指针、NDT对象的共享指针等参数，并创建了订阅、发布、服务客户端和定时器。处理EKF里程计数据的回调函数会计算当前位置与上次更新位置之间的距离，并输出错误信息，如果超过了阈值。地图更新定时器的回调函数会判断是否应该更新地图，如果是，则开始更新地图。更新地图数据的函数会设置请求参数、发送请求给地图加载服务，然后处理响应并更新NDT对象。最后，它还实现了发布已加载的点云地图的函数。

/// @brief 计算两个点之间的欧氏距离
/// @tparam T 
/// @tparam U 
/// @param p1 第一个点
/// @param p2 第二个点
/// @return 
template <typename T, typename U>
double norm_xy(const T p1, const U p2)
{double dx = p1.x - p2.x;double dy = p1.y - p2.y;return std::sqrt(dx * dx + dy * dy);
}/// @brief 构造函数，初始化地图更新模块
/// @param node 节点指针
/// @param ndt_ptr_mutex 互斥锁指针，用于保护NDT指针
/// @param ndt_ptr 指向NDT对象的共享指针
/// @param tf2_listener_module TF2监听模块的共享指针 
/// @param map_frame 地图坐标系 
/// @param main_callback_group 主回调函数组
MapUpdateModule::MapUpdateModule(rclcpp::Node * node, std::mutex * ndt_ptr_mutex,std::shared_ptr<NormalDistributionsTransform> ndt_ptr,std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,rclcpp::CallbackGroup::SharedPtr main_callback_group)
: ndt_ptr_(std::move(ndt_ptr)),ndt_ptr_mutex_(ndt_ptr_mutex),map_frame_(std::move(map_frame)),logger_(node->get_logger()),clock_(node->get_clock()),tf2_listener_module_(std::move(tf2_listener_module)),dynamic_map_loading_update_distance_(node->declare_parameter<double>("dynamic_map_loading_update_distance")),dynamic_map_loading_map_radius_(node->declare_parameter<double>("dynamic_map_loading_map_radius")),lidar_radius_(node->declare_parameter<double>("lidar_radius"))
{auto main_sub_opt = rclcpp::SubscriptionOptions();main_sub_opt.callback_group = main_callback_group;map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//创建回调函数组ekf_odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>("ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1),main_sub_opt);//订阅EKF里程计loaded_pcd_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local());//发布已加载的点云地图pcd_loader_client_ =node->create_client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>("pcd_loader_service");//创建点云地图加载服务客户端double map_update_dt = 1.0;auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(map_update_dt));//地图更新周期map_update_timer_ = rclcpp::create_timer(node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this),map_callback_group_);//创建定时器
}/// @brief 处理EKF里程计数据的回调函数
/// @param odom_ptr 接收到的EKF里程计数据指针odom_ptr
void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr)
{current_position_ = odom_ptr->pose.pose.position;if (last_update_position_ == std::nullopt) {return;}//计算当前位置与上次更新位置之间的距离double distance = norm_xy(current_position_.value(), last_update_position_.value());if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) {//如果距离超过一定阈值，则输出错误信息RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up.");}
}/// @brief 地图更新定时器的回调函数
void MapUpdateModule::map_update_timer_callback()
{if (current_position_ == std::nullopt) {RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1,"Cannot find the reference position for map update. Please check if the EKF odometry is ""provided to NDT.");return;}if (last_update_position_ == std::nullopt) return;// continue only if we should update the map// 检查是否应该更新地图if (should_update_map(current_position_.value())) {RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)");update_map(current_position_.value());last_update_position_ = current_position_;}
}/// @brief 判断是否应该更新地图
/// @param position 当前位置
/// @return 
bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const
{//计算当前位置与上次更新位置之间的距离，如果距离超过一定阈值，则返回true，否则返回false。if (last_update_position_ == std::nullopt) return false;double distance = norm_xy(position, last_update_position_.value());return distance > dynamic_map_loading_update_distance_;
}/// @brief 更新地图数据
/// @param position 当前位置
void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
{//设置请求参数auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();request->area.center_x = static_cast<float>(position.x);request->area.center_y = static_cast<float>(position.y);request->area.radius = static_cast<float>(dynamic_map_loading_map_radius_);//设置加载半径request->cached_ids = ndt_ptr_->getCurrentMapIDs();//获取当前地图ID//等待服务可用while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {RCLCPP_INFO(logger_,"Waiting for pcd loader service. Check if the enable_differential_load in ""pointcloud_map_loader is set `true`.");}// send a request to map_loader//发送请求auto result{pcd_loader_client_->async_send_request(request,[](rclcpp::Client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedFuture) {})};// 设置超时时间std::future_status status = result.wait_for(std::chrono::seconds(0));while (status != std::future_status::ready) {RCLCPP_INFO(logger_, "waiting response");if (!rclcpp::ok()) {return;}status = result.wait_for(std::chrono::seconds(1));}//处理响应，更新地图update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove);last_update_position_ = position;
}/// @brief 更新NDT对象
/// @param maps_to_add 要添加的地图数据
/// @param map_ids_to_remove 要移除的地图数据的ID
void MapUpdateModule::update_ndt(const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & maps_to_add,const std::vector<std::string> & map_ids_to_remove)
{RCLCPP_INFO(logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size());if (maps_to_add.empty() && map_ids_to_remove.empty()) {//如果没有要添加的地图数据，也没有要移除的地图数据，则直接返回RCLCPP_INFO(logger_, "Skip map update");return;}const auto exe_start_time = std::chrono::system_clock::now();NormalDistributionsTransform backup_ndt = *ndt_ptr_;//备份NDT对象// Add pcdfor (const auto & map_to_add : maps_to_add) {//遍历要添加的地图数据pcl::shared_ptr<pcl::PointCloud<PointTarget>> map_points_ptr(new pcl::PointCloud<PointTarget>);pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr);backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id);//map_to_add.cell_id指定了map_points_ptr应该被添加到backup_ndt中的哪个单元格（cell）中}// Remove pcdfor (const std::string & map_id_to_remove : map_ids_to_remove) {backup_ndt.removeTarget(map_id_to_remove);//从backup_ndt中移除map_id_to_remove指定的单元格}backup_ndt.createVoxelKdtree();//创建体素树const auto exe_end_time = std::chrono::system_clock::now();const auto duration_micro_sec =std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);// swap(*ndt_ptr_mutex_).lock();// ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should// ideally be avoided. But I will leave this for now since I cannot come up with a solution other// than using pointer of pointer.*ndt_ptr_ = backup_ndt;(*ndt_ptr_mutex_).unlock();publish_partial_pcd_map();//发布已加载的点云地图
}/// @brief 发布已加载的点云地图
void MapUpdateModule::publish_partial_pcd_map()
{pcl::PointCloud<PointTarget> map_pcl = ndt_ptr_->getVoxelPCD();sensor_msgs::msg::PointCloud2 map_msg;pcl::toROSMsg(map_pcl, map_msg);map_msg.header.frame_id = "map";loaded_pcd_pub_->publish(map_msg);
}

1.4 ndt_scan_matcher_core.cpp

这段代码是一个基于正态分布变换的NDT（Normal Distributions Transform）扫描匹配算法。该算法接收传感器点云数据和初始姿态，并执行扫描匹配以估计自车的姿态。算法首先对初始姿态进行TPE（Tree-structured Parzen Estimator）优化，生成一组粒子以覆盖姿态空间。然后，对每个粒子执行NDT扫描匹配，得到匹配结果姿态和变换矩阵数组。接着，根据匹配结果评分，选择最优的姿态作为最终结果。最终结果包含估计的姿态以及姿态的协方差矩阵，表示姿态估计的不确定性。同时，算法会发布匹配结果的可视化标记和匹配结果的点云数据。

…详情请参照古月居

这篇关于Autoware 定位之ndt定位（八）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/1146984。

相关文章

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
js定位navigator.geolocation

js定位navigator.geolocation

一、简介 　　html5为window.navigator提供了geolocation属性，用于获取基于浏览器的当前用户地理位置。 　　window.navigator.geolocation提供了3个方法分别是： void getCurrentPosition(onSuccess,onError,options);//获取用户当前位置int watchCurrentPosition(
阅读更多...
flume系列之：记录一次flume agent进程被异常oom kill -9的原因定位

flume系列之：记录一次flume agent进程被异常oom kill -9的原因定位

flume系列之：记录一次flume agent进程被异常oom kill -9的原因定位 一、背景二、定位问题三、解决方法 一、背景 flume系列之：定位flume没有关闭某个时间点生成的tmp文件的原因，并制定解决方案在博主上面这篇文章的基础上，在机器内存、cpu资源、flume agent资源都足够的情况下，flume agent又出现了tmp文件无法关闭的情况 二、
阅读更多...
一次生产环境大量CLOSE_WAIT导致服务无法访问的定位过程

一次生产环境大量CLOSE_WAIT导致服务无法访问的定位过程

1.症状 生产环境的一个服务突然无法访问，服务的交互过程如下所示： 所有的请求都是通过网关进入，之后分发到后端服务。 现在的情况是用户服务无法访问商旅服务，网关有大量java.net.SocketTimeoutException: Read timed out报错日志，商旅服务也不断有日志打印，大多是回调和定时任务日志，所以故障点在网关和商旅服务，大概率是商旅服务无法访问导致网关超时。 后
阅读更多...
定位cpu占用过高的线程和对应的方法

定位cpu占用过高的线程和对应的方法

如何定位cpu占用过高的线程和对应的方法？ 主要是通过线程id找到对应的方法。 1 查询某个用户cpu占用最高的进程号 top -u 用户名 2 查询这个进程中占用cpu最高的线程号 top –p 进程号-H    3 查询到进程id后把进程相关的代码打印到jstack文件 jstack -l pid > jstack.txt 4 在jstack文件中通过16进制的线程id搜索到
阅读更多...
python 定位元素

python 定位元素

获取元素列表gg = driver.find_elements_by_css_selector("div.offer-attr-item")循环元素列表for g in range(0,len(gg)):获取元素列表下面的元素，==定位元素后，可以继续定位gname = driver.find_elements_by_css_selector("div.offer-attr-item")[
阅读更多...
物联网-标识定位

物联网-标识定位

标识技术 一维条码 商品条码-UPC EAN ISBN码 977-期刊ISSN 二维码
阅读更多...
镭射定位灯激光定位使用注意事项?

镭射定位灯激光定位使用注意事项?

在现代工业、建筑测量、舞台设计以及科研实验等领域，镭射定位灯(常称激光定位器)因其高精度、远射程和直观性而得到广泛应用。然而，激光作为一种高强度光束，其使用若不当，不仅可能损害设备，还可能对人体健康造成危害。因此，掌握镭射定位灯激光定位的正确使用方法及注意事项至关重要。下面就跟着鑫优威一起来了解一下。 　　一、安全为先，做好防护 　　首先，操作者必须佩戴符合标准的激光防护眼镜，以防止激光直
阅读更多...
mysql快速定位cpu 占比过高的sql语句

mysql快速定位cpu 占比过高的sql语句

mysql快速定位cpu 占比过高的sql语句 当MySQL数据库的CPU使用率异常升高时，定位导致问题的SQL语句可以通过以下步骤进行 1、使用top命令找出mysl进程中占用CPU靠前的线程 #找出mysql 的进程号ps -ef | grep mysql#根据进程号，找出占用CPU靠前的线程号top -H -p <mysqld进程id> top 中，按大写的P ，进行CPU
阅读更多...
F12抓包04：(核心功能)Network接口抓包、定位缺陷

F12抓包04：(核心功能)Network接口抓包、定位缺陷

课程大纲 一、录制请求 ① tab导航选择“网络”（Network），即可进入网络抓包界面，进入界面默认开启录制模式，显示浏览器当前标签页的请求列表。 ② 查看请求列表，包含了当前标签页执行的所有请求和下载的资源，列表显示每条请求的相应内容。 还可以在字段行单击右键，勾选想要查看的字段。 ③ 单击列表项的“名称”，可以查看请求的详细内容。接口请
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2