在ROS下使用Cartographer的纯定位模式，并实时获取定位位姿数据

文章目录

前言

Cartographer提供了一种纯定位模式，在实际工程中有很多应用。本文记录在Gazebo仿真环境下，使用turtlebot3在cartographer的纯定位模式下，发布实时定位数据的过程。

一、Cartographer纯定位模式的配置

1.启动纯定位模式配置

修改你的.lua文件，添加以下橙色内容颜色显示内容。

2.加载定位地图，并删除cartographer生成地图节点

在你的.launch文件中添加map_server 节点，并删除cartographer_occupancy_grid_node节点，如下

<node name="map_server" pkg="map_server" type="map_server" 
args="/home/{yourname}/catkin_ws/map/{yourmapname}.yaml" />
 <!-- cartographer_node -->
  <node pkg="cartographer_ros" type="cartographer_node" name="cartographer_node" 
        args="-configuration_directory $(find turtlebot3_slam)/config
              -configuration_basename turtlebot3_lds_2d_pureLocation.lua
          -load_state_filename /home/{yourname}/catkin_ws/map/{yourmapname}.pbstream" 
        output="screen">
  <!-- cartographer_occupancy_grid_node 
  <node pkg="cartographer_ros" type="cartographer_occupancy_grid_node"
        name="cartographer_occupancy_grid_node" 
        args="-resolution 0.05 " />
-->

二、实时获取定位数据

cartographer纯定位模式下获取定位数据，就是获取map坐标到base_link的tf。

1.新建一个节点发布定位数据

在你的工作区间的软件包的src文件夹下添加一个CartoTransferPose_main.cc文件，内容如下。

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Pose2D.h>
#include <fstream>
#include <iostream>
#include <cmath>

using namespace std;
double x,y,z, qx,qy,qz, qw;
double theta;
geometry_msgs::Pose2D pos_now;

int main(int argc, char** argv){
  ros::init(argc, argv, "tf_Pose_Publisher");
 
  ros::NodeHandle node;
  ros::Publisher _pose_pub=node.advertise<geometry_msgs::Pose2D>("pose_data", 10);
  tf::StampedTransform transform;
  tf::TransformListener listener;
  ros::Rate rate(10.0);

  while (ros::ok()){
    ros::Time start = ros::Time::now();
    cout << "StartTime:"<< start << endl;
    tf::StampedTransform transform;
    try{
        //得到坐map和坐标base_link之间的关系
      listener.waitForTransform("map","base_link",  ros::Time(0), ros::Duration(3.0));
      listener.lookupTransform("map", "base_link", ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
 
    x=transform.getOrigin().x();
    y=transform.getOrigin().y();
    z=transform.getOrigin().z();
  
    tf::Quaternion q = transform.getRotation();
    qx = q.x();
    qy = q.y();
     qz = q.z();
     qw = q.w();

  pos_now.x = transform.getOrigin().x();
  pos_now.y =transform.getOrigin().y();
  pos_now.theta = tf::getYaw(q);
  _pose_pub.publish(pos_now);
    printf("x: %f, y: %f, z: %f, qx: %f,qy: %f,qz: %f, qw: %f, theta: %f\n",x,y,z,qx,qy,qz,qw,pos_now.theta);
    rate.sleep();
    
  ros::Time end = ros::Time::now();
  cout << "EndTime:"<<end << endl;
  }
  return 0;
};

2.修改makelist文件并编译

添加以下代码到Cmakelist文件

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf
)add_executable(CartoTransferPose_main src/CartoTransferPose_main.cc)target_link_libraries(CartoTransferPose_main ${catkin_LIBRARIES})

保存并编译。

3.启动仿真环境并启动节点

首先打开gazebo环境并按之前修改配置启动纯定位模式。
在命令行输入rostopic echo /pose_data。 这里发布的即位姿数据。

在命令行用rosrun启动CartoTransferPose_main节点，出现以下界面。其中x,y,theta即为位姿数据，频率为10hz。

总结

本文配置了Cartographer的纯定位模式，并获取了定位数据，为后续应用做好了铺垫。