ROS小车记录系列（四）利用cartographer定位，修改源码发布pose话题

最新推荐文章于 2022-06-18 12:18:24 发布

Ai_Smith

最新推荐文章于 2022-06-18 12:18:24 发布

阅读量5.7k

点赞数 24

分类专栏： ROS 文章标签： cartographer定位

本文链接：https://blog.csdn.net/Ai_Smith/article/details/108125861

版权

ROS 专栏收录该内容

9 篇文章 30 订阅

订阅专栏

（四）利用cartographer定位

__半个月没有更新了，一直在解决建图效果问题，因为是用的自己的IMU，误差有点大（-_-!!!），陆续买了两个imu，误差依旧大，转几圈都找不到北了。加上我的odom计算是依赖IMU的，最终结果就是一片模糊，还不如纯激光雷达效果。最终还是狠下心，花了2千，买了个精度稳定性都靠谱的。
__定位这事本来想用amcl做，后来听说现在大家都用cartographer了，我也跟着做一下，按照这篇博客一通配置，果然跑起来了，看到定位效果确实好啊，又准稳定性又好。但是，重点来了，但是定位后我要pose信息啊，不然后面怎么导航，查看topic list，卵，没有pose相关话题。。。这篇文章就记录这一骚操作，从cartographer中发布出pose信息，简单两步走。
（更正1、实际上move_base是通过TF树查询位置的，不需要单独再发布pose信息，这里发出来就留给上位机界面展示车子位置使用吧;
更正2：使用这种方式获取位姿信息，只有在cartographer配置参数provide_odom_frame = true时才有效，而如果使用odom数据建图、导航，该参数需要设置为false，所以下述方法无效，发布的信息是错误的。
）

第一，cartographer源码目录，打开src/cartographer_ros/cartographer_ros/cartographer_ros/node.h文件，182行插入一句：

::ros::Publisher _pose_pub;

第二，cartographer源码目录，打开src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc文件，修改后完整include如下：

#include "Eigen/Core"
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros_msgs/StatusCode.h"
#include "cartographer_ros_msgs/StatusResponse.h"
#include "glog/logging.h"
#include "nav_msgs/Odometry.h"
#include "ros/serialization.h"
#include "sensor_msgs/PointCloud2.h"
#include "tf2_eigen/tf2_eigen.h"
#include "visualization_msgs/MarkerArray.h"
#include "tf/transform_broadcaster.h"
#include <geometry_msgs/Pose2D.h>

182行找到函数：

void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event){

下面插入一句：

_pose_pub = node_handle_.advertise<geometry_msgs::Pose2D>("pose_nav", 10);

235行插入：

geometry_msgs::Pose2D pos_now;

264行插入：

geometry_msgs::Transform transform = ToGeometryMsgTransform(
                  tracking_to_map * (*trajectory_state.published_to_tracking));
pos_now.x = static_cast<double>(transform.translation.x);
pos_now.y = static_cast<double>(transform.translation.y);
pos_now.theta = tf::getYaw(transform.rotation);
_pose_pub.publish(pos_now);

好了，最终重新编译一下cartographer：

catkin_make_isolated --install --use-ninja

现在，启动机器，启动建图或者定位，查看下topic，惊不惊喜，意不意外，多了个topic：pose_nav，查看下，刷刷的发布着位置信息，频率100Hz。
|
|
|
|
|
PS：还是贴个完整的修改后函数吧：

void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
  carto::common::MutexLocker lock(&mutex_);
  _pose_pub = node_handle_.advertise<geometry_msgs::Pose2D>("pose_nav", 10);
  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
    const auto& trajectory_state = entry.second;

    auto& extrapolator = extrapolators_.at(entry.first);
    // We only publish a point cloud if it has changed. It is not needed at high
    // frequency, and republishing it would be computationally wasteful.
    if (trajectory_state.local_slam_data->time !=
        extrapolator.GetLastPoseTime()) {
      if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
        // TODO(gaschler): Consider using other message without time
        // information.
        carto::sensor::TimedPointCloud point_cloud;
        point_cloud.reserve(trajectory_state.local_slam_data
                                ->range_data_in_local.returns.size());
        for (const Eigen::Vector3f point :
             trajectory_state.local_slam_data->range_data_in_local.returns) {
          Eigen::Vector4f point_time;
          point_time << point, 0.f;
          point_cloud.push_back(point_time);
        }
        scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
            carto::common::ToUniversal(trajectory_state.local_slam_data->time),
            node_options_.map_frame,
            carto::sensor::TransformTimedPointCloud(
                point_cloud, trajectory_state.local_to_map.cast<float>())));
      }
      extrapolator.AddPose(trajectory_state.local_slam_data->time,
                           trajectory_state.local_slam_data->local_pose);
    }

    geometry_msgs::TransformStamped stamped_transform;
    // If we do not publish a new point cloud, we still allow time of the
    // published poses to advance. If we already know a newer pose, we use its
    // time instead. Since tf knows how to interpolate, providing newer
    // information is better.
    const ::cartographer::common::Time now = std::max(
        FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
    stamped_transform.header.stamp = ToRos(now);

    const Rigid3d tracking_to_local = [&] {
      if (trajectory_state.trajectory_options.publish_frame_projected_to_2d) {
        return carto::transform::Embed3D(
            carto::transform::Project2D(extrapolator.ExtrapolatePose(now)));
      }
      return extrapolator.ExtrapolatePose(now);
    }();

    const Rigid3d tracking_to_map =
        trajectory_state.local_to_map * tracking_to_local;

    geometry_msgs::Pose2D pos_now;
    if (trajectory_state.published_to_tracking != nullptr) {
      if (trajectory_state.trajectory_options.provide_odom_frame) {
        std::vector<geometry_msgs::TransformStamped> stamped_transforms;

        stamped_transform.header.frame_id = node_options_.map_frame;
        stamped_transform.child_frame_id =
            trajectory_state.trajectory_options.odom_frame;
        stamped_transform.transform =
            ToGeometryMsgTransform(trajectory_state.local_to_map);
        stamped_transforms.push_back(stamped_transform);

        stamped_transform.header.frame_id =
            trajectory_state.trajectory_options.odom_frame;
        stamped_transform.child_frame_id =
            trajectory_state.trajectory_options.published_frame;
        stamped_transform.transform = ToGeometryMsgTransform(
            tracking_to_local * (*trajectory_state.published_to_tracking));
        stamped_transforms.push_back(stamped_transform);

        tf_broadcaster_.sendTransform(stamped_transforms);
      } else {
        stamped_transform.header.frame_id = node_options_.map_frame;
        stamped_transform.child_frame_id =
            trajectory_state.trajectory_options.published_frame;
        stamped_transform.transform = ToGeometryMsgTransform(
            tracking_to_map * (*trajectory_state.published_to_tracking));
        tf_broadcaster_.sendTransform(stamped_transform);
      }

      geometry_msgs::Transform transform = ToGeometryMsgTransform(
                tracking_to_map * (*trajectory_state.published_to_tracking));

      pos_now.x = static_cast<double>(transform.translation.x);
      pos_now.y = static_cast<double>(transform.translation.y);
      pos_now.theta = tf::getYaw(transform.rotation);
      _pose_pub.publish(pos_now);
    }
  }
}