【移动机器人技术】cartographer定位模式下的功能开发(二):订阅/initialpose
google开发的cartographer开源包,既可以用来做SLAM建图,又可以用来做纯定位。在实时定位工作过程中,若从外部信息获取到了机器人的准确位姿,如何像move_base中的amcl模块一样,通过订阅/initialpose话题达到修正定位结果的目的呢?本文记录了整个操作流程。2 实时修正定位位姿2.1 编写回调函数在文件node_main.cc的头部编写回调函数:函数中订阅了/init
·
google开发的cartographer开源包,既可以用来做SLAM建图,又可以用来做纯定位。
在实时定位工作过程中,若从外部信息获取到了机器人的准确位姿,如何像move_base中的amcl模块一样,通过订阅/initialpose话题达到修正定位结果的目的呢?
本文记录了整个操作流程。
2 实时修正定位位姿
2.1 编写回调函数
在文件node_main.cc的头部编写回调函数:
函数中订阅了/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
- 首先定义全局变量
在文件node_main.cc头部位置
cartographer_ros::Node* node_handle;
cartographer_ros::TrajectoryOptions* trajectory_options_handle;
- 初始化全局变量
在文件node_main.cc的void Run()中:
trajectory_options_handle = &(trajectory_options);
node_handle = &(node);
- 订阅话题
在文件node_main.cc的void Run()函数中添加如下代码:
ros::Subscriber initPose_sub = node.node_handle()->subscribe("/initialpose", 1, Reset_InitPose_callback);
更多推荐
已为社区贡献1条内容
所有评论(0)