Method process

track_odometry/src/tf_projection_node.cpp:98–157

    pnh_.param("align_all_posture_to_source", align_all_posture_to_source_, false);
  }
  void process()
  {
    tf2::Stamped<tf2::Transform> trans;
    tf2::Stamped<tf2::Transform> trans_target;
    try
    {
      tf2::fromMsg(
          tf_buffer_.lookupTransform(projection_surface_frame_, source_frame_, ros::Time(0), ros::Duration(0.1)),
          trans);
      tf2::fromMsg(
          tf_buffer_.lookupTransform(parent_frame_, projection_surface_frame_, trans.stamp_, ros::Duration(0.1)),
          trans_target);
    }
    catch (tf2::TransformException& e)
    {
      ROS_WARN_THROTTLE(1.0, "%s", e.what());
      return;
    }

    if (!trans.stamp_.isZero())
      trans.stamp_ += ros::Duration(tf_tolerance_);

    if (project_posture_)
    {
      if (align_all_posture_to_source_)
      {
        const tf2::Quaternion rot(trans.getRotation());
        const tf2::Quaternion rot_yaw(tf2::Vector3(0.0, 0.0, 1.0), tf2::getYaw(rot));
        const tf2::Transform rot_inv(rot_yaw * rot.inverse());
        trans.setData(rot_inv * trans);
      }
      else
      {
        const float yaw = tf2::getYaw(trans.getRotation());
        trans.setRotation(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
      }
    }

    const tf2::Stamped<tf2::Transform> result(
        track_odometry::projectTranslation(trans, trans_target),
        trans.stamp_,
        parent_frame_);

    geometry_msgs::TransformStamped trans_out = tf2::toMsg(result);
    if (flat_)
    {
      const double yaw = tf2::getYaw(trans_out.transform.rotation);
      trans_out.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
    }
    trans_out.child_frame_id = projected_frame_;

    if (trans.stamp_.isZero())
    {
      tf_static_broadcaster_.sendTransform(trans_out);
    }
    else
    {
      tf_broadcaster_.sendTransform(trans_out);