【移动机器人技术】cartographer定位模式下的功能开发（二）：订阅initialpose

【移动机器⼈技术】cartographer定位模式下的功能开发（⼆）：订阅

initialpose

google开发的cartographer开源包，既可以⽤来做SLAM建图，⼜可以⽤来做纯定位。

在实时定位⼯作过程中，若从外部信息获取到了机器⼈的准确位姿，如何像move_base中的amcl模块⼀样，通过订阅/initialpose话题达到修正定位结果的⽬的呢？

本⽂记录了整个操作流程。

2 实时修正定位位姿

2.1 编写回调函数

在⽂件的头部编写回调函数：声音检测电路

体内卫生巾函数中订阅了/initialpose后，结束了当前活动的轨迹，设置轨迹配置选项中的起点为话题值，重新开启节点。

void Reset_InitPose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) {

// 关闭当前运⾏的Trajectories

node_handle->FinishAllTrajectories();

分体挂壁式空调

// 给轨迹设置起点 msg->pose.pose

// start trajectory with initial pose

*trajectory_options_handle->trajectory_builder_options.mutable_initial_trajectory_pose()->mutable_relative_pose()

= cartographer::transform::ToProto(cartographer_ros::ToRigid3d(msg->pose.pose));

// 重新开启Trajectory

if (FLAGS_start_trajectory_with_default_topics)

{

node_handle->StartTrajectoryWithDefaultTopics(*trajectory_options_handle);

汽车铆钉

}

伞齿轮传动

}

提⽰：如编译报错，提⽰“ error: ‘ToRigid3d’ is not a member of ‘cartographer_ros’ ”，则增加头⽂件包含，#include “cartographer_ros/msg_conversion.h”。

2.2 订阅/initialpose

⾸先定义全局变量

在⽂件头部位置

cartographer_ros::Node* node_handle;

cartographer_ros::TrajectoryOptions* trajectory_options_handle;

初始化全局变量

在⽂件的void Run()中：

trajectory_options_handle = &(trajectory_options);

node_handle = &(node);

文具盒生产过程

订阅话题

在⽂件的void Run()函数中添加如下代码：

ros::Subscriber initPose_sub = de_handle()->subscribe("/initialpose", 1, Reset_InitPose_callback);

本文发布于:2024-09-22 22:24:59，感谢您对本站的认可！

标签：定位函数轨迹话题

留言与评论（共有 0 条评论）