ros bag包坐标系偏移，对bag包坐标系进行修正并重新录包(一)

背景描述

• 使用激光雷达在录制点云bag包的过程中，因为激光雷达晃动，坐标系倾斜。导致影响后续的点云标注和模型训练。
  
• 现在需要将录制的bag包进行处理，调整点云坐标系，使之与base_link坐标平行(点云地面与base_link坐标平面平行)。
• 参考如下项目代码：tunable_static_tf_broadcaster

使用步骤

• 项目代码结构很简单。源代码.cpp主要看tunable_static_tf_broadcaster.cpp。
• 使用方法：

cd ~
mkdir tunable_static_tf_broadcaster
cd tunable_static_tf_broadcaster
mkdir src
cd src
git clone https://github.com/jianbuzhang/tunable_static_tf_broadcaster.git
cd ..
catkin_make
source devel/setup.bash
roslaunch tunable_static_tf_broadcaster sample.launch

• 启动项目后，弹出下面的界面，选中arm，会有x,y,z,r,p,y六个调节值。
  
• 同时播放坐标系有倾斜的bag包，打开arm和base_link坐标系。并将Fixed Frame设置为arm。如下图：
  
  此时调整项目gui的x,y,z,r,p,y。使点云地面相对于arm坐标水平。
  记录gui界面的值。
  
  至此，该项目所有步骤完成。

#include <string>
#include <tf2/LinearMath/Quaternion.h>
#include "tunable_static_tf_broadcaster/tunable_static_tf_broadcaster.h"
#include "tunable_static_tf_broadcaster/TfConfig.h"

namespace tunable_static_tf_broadcaster
{
TunableStaticTfBroadcaster::TunableStaticTfBroadcaster() : private_nh_("~")
{
  // Initialize transform
  transform_.transform.translation.x = 0.0;
  transform_.transform.translation.y = 0.0;
  transform_.transform.translation.z = 0.0;
  tf2::Quaternion quaternion;
  quaternion.setRPY(0.0, 0.0, 0.0);
  transform_.transform.rotation.x = quaternion.x();
  transform_.transform.rotation.y = quaternion.y();
  transform_.transform.rotation.z = quaternion.z();
  transform_.transform.rotation.w = quaternion.w();

  // Get configuration from param
  publish_rate_ = private_nh_.param<int>("rate", 10);
  transform_.header.frame_id = private_nh_.param<std::string>("header_frame", "world");
  transform_.child_frame_id = private_nh_.param<std::string>("child_frame", "base_link");

  ROS_DEBUG("[tunable_static_tf_broadcaster] Setting parameters... {rate: %d, header_frame: %s, child_frame: %s}",
            publish_rate_, transform_.header.frame_id.c_str(), transform_.child_frame_id.c_str());

  // Get if static param is set
  if (private_nh_.hasParam("tf_x") || private_nh_.hasParam("tf_y") || private_nh_.hasParam("tf_z") ||
      private_nh_.hasParam("tf_roll") || private_nh_.hasParam("tf_pitch") || private_nh_.hasParam("tf_yaw"))
  {
    has_initial_static_params_ = true;
  }
  else
  {
    has_initial_static_params_ = false;
  }

  // Setup dynamic reconfigure
  dynamic_reconfigure_server_ = std::make_shared<dynamic_reconfigure::Server<TfConfig> >();
  dynamic_reconfigure::Server<TfConfig>::CallbackType f;
  f = boost::bind(&TunableStaticTfBroadcaster::dynamicReconfigureCallback, this, _1, _2);
  dynamic_reconfigure_server_->setCallback(f);
}

TunableStaticTfBroadcaster::~TunableStaticTfBroadcaster()
{
  // Delete params for another run
  if (!is_valid_config_received_)
  {
    private_nh_.deleteParam("tf_x");
    private_nh_.deleteParam("tf_y");
    private_nh_.deleteParam("tf_z");
    private_nh_.deleteParam("tf_roll");
    private_nh_.deleteParam("tf_pitch");
    private_nh_.deleteParam("tf_yaw");
  }
}

void TunableStaticTfBroadcaster::run()
{
  ros::Rate rate(publish_rate_);
  ros::Duration duration(1.0 / publish_rate_);

  while (private_nh_.ok())
  {
    // Send transform
    if (is_valid_config_received_)
    {
      transform_.header.stamp = ros::Time::now() + duration;  // Set future time to allow
                                                              // slower sending w/o other node's timeout.
      broadcaster_.sendTransform(transform_);
    }
    else
    {
      ROS_WARN_THROTTLE(3.0, "[tunable_static_tf_broadcaster] Configuration not received. waiting...");
    }

    ros::spinOnce();
    rate.sleep();
  }
}

void TunableStaticTfBroadcaster::dynamicReconfigureCallback(TfConfig& config, uint32_t level)
{
  if (is_first_config_callback_)
  {
    // If it was first calling, there are cases that it has static param value or not.
    if (has_initial_static_params_)
    {
      // Get static param value
      transform_.transform.translation.x = config.tf_x;
      transform_.transform.translation.y = config.tf_y;
      transform_.transform.translation.z = config.tf_z;
      tf2::Quaternion quaternion;
      quaternion.setRPY(config.tf_roll, config.tf_pitch, config.tf_yaw);
      transform_.transform.rotation.x = quaternion.x();
      transform_.transform.rotation.y = quaternion.y();
      transform_.transform.rotation.z = quaternion.z();
      transform_.transform.rotation.w = quaternion.w();
      is_valid_config_received_ = true;

      ROS_DEBUG("[tunable_static_tf_broadcaster] Setting parameters by static params... {x: %lf y: %lf z: %lf roll: "
                "%lf pitch: %lf yaw: %lf}",
                config.tf_x, config.tf_y, config.tf_z, config.tf_roll, config.tf_pitch, config.tf_yaw);
    }
    else
    {
      // Static param is not set. Wait for dynamic configuration.
      is_valid_config_received_ = false;
    }
    is_first_config_callback_ = false;
  }
  else
  {
    // Not first calling
    transform_.transform.translation.x = config.tf_x;
    transform_.transform.translation.y = config.tf_y;
    transform_.transform.translation.z = config.tf_z;
    tf2::Quaternion quaternion;
    quaternion.setRPY(config.tf_roll, config.tf_pitch, config.tf_yaw);
    transform_.transform.rotation.x = quaternion.x();
    transform_.transform.rotation.y = quaternion.y();
    transform_.transform.rotation.z = quaternion.z();
    transform_.transform.rotation.w = quaternion.w();
    is_valid_config_received_ = true;

    ROS_DEBUG("[tunable_static_tf_broadcaster] Setting dynamic parameters... {x: %lf y: %lf z: %lf roll: %lf pitch: "
              "%lf yaw: %lf}",
              config.tf_x, config.tf_y, config.tf_z, config.tf_roll, config.tf_pitch, config.tf_yaw);
  }
}

}  // namespace tunable_static_tf_broadcaster

后续

• 该文章为第一章节。bag包处理第二节请看下一篇。