LaserScan transform

[facontidavide]

Hi,

using LaserScan I noticed that the transform operation is potentially applied twice, once in the laserScanCallback and later, before, drawing, in Transform().

I am not sure I understand the reasoning behind repeating the transform in two different places. In my case, the GetTransform succeed in laserScanCallback and fails in Tranform(). Therefore Draw() will not display it. https://github.com/swri-robotics/mapviz/blob/indigo-devel/mapviz_plugins/src/laserscan_plugin.cpp#L452

shouldn't Transform() be modified as follows:

  void LaserScanPlugin::Transform()
  {
    std::deque<Scan>::iterator scan_it = scans_.begin();
    for (; scan_it != scans_.end(); ++scan_it)
    {
      Scan& scan = *scan_it;

      swri_transform_util::Transform transform;

      if( !scan.transformed )
      {
        bool was_using_latest_transforms = this->use_latest_transforms_;
        this->use_latest_transforms_ = false;
        if (GetTransform(scan.source_frame_, scan.stamp, transform))
        {
          scan.transformed = true;
          std::vector<StampedPoint>::iterator point_it = scan.points.begin();
          for (; point_it != scan.points.end(); ++point_it)
          {
            point_it->transformed_point = transform * point_it->point;
          }
        }
        this->use_latest_transforms_ = was_using_latest_transforms;    
      }
    }
    // Z color is based on transformed color, so it is dependent on the
    // transform
    if (ui_.color_transformer->currentIndex() == COLOR_Z)
    {
      UpdateColors();
    }
  }

Cheers

[facontidavide]

any feedback?

[malban]

That makes sense to me. It looks like there is an inconsistency in the two code paths though. In the callback the use_latest_transforms flag is left whatever it is, but in the Transform function it is set to false explicitly. That may be why you are seeing it fail in one and not the other.

I think in almost every conceivable case the use_latest_transforms flag should be false for this type of data. The use_latest_transforms flag ignores the stamp of the data resulting in the points being transformed to the current sensor position instead of the sensor position when the data was captured. So, in addition to your suggested change, we would also want to make sure the correct transform is applied in the callback.

[facontidavide]

About that "almost every conceivable case"... ;) My problem is that unless I use latest transform, my laser fails to transform.

I am reproducing a rosbag with the option --clock and use_sim_time=true.

I understand your reasoning about using timestamp of the message but I am not sure how deal with rosbags? It would be nice if the README.md could mention this.

[facontidavide]

One more thing. Would it be acceptable to try transforming first with use_latest_transforms = false and only if that fails, try with use_latest_transforms = true ?

Something like this:

  bool LaserScanPlugin::GetScanTransform(const Scan& scan, swri_transform_util::Transform& transform)
  {
      bool was_using_latest_transforms = this->use_latest_transforms_;
      //Try first with use_latest_transforms_ = false
      this->use_latest_transforms_ = false;
      bool has_tranform = GetTransform(scan.source_frame_, scan.stamp, transform);
      if( !has_tranform && was_using_latest_transforms)
      {
          //If failed use_latest_transforms_ = true
          this->use_latest_transforms_ = true;
          has_tranform = GetTransform(scan.source_frame_, scan.stamp, transform);
      }
      this->use_latest_transforms_ = was_using_latest_transforms;
      return has_tranform;
  }

[malban]

I think the first step in your case is to find out why the stamped transformation is not working. Our experience has been that bag playback with TF doesn't always work as expected out-of-the-box because of how bags are recorded and how TF works.

1. Bags recording doesn't guarantee topics including TF are in the same order as they were originally published. This is especially true when larger sensor messages are involved. They are also not ordered by header stamp.
2. TF will interpolate, but not extrapolate, so if a sensor message in the bag is published before the necessary TF messages have, the transform will likely fail unless a sufficient wait is provided.

TF can be quite sensitive in this respect. Take the case where messages are published in the right order:

1. TF message at time t=0.0
2. Sensor message at time t=0.05
3. TF message at time t=0.1

If you receive the sensor message before the second transform, which is likely, the transform request will fail unless a wait of at least 0.05 is allowed. This is why it is often necessary to publish static transformed at a very high rate ~100Hz if using the tf/static_transform_publisher.

There is a trick that can be used to avoid having to publish static transforms at such a high rate. And that is to give them a timestamp slightly in the future when publishing them. That way the TF listeners will not have an extrapolation issue, their buffer has data from -9 sec to +1 sec instead of -10 sec to 0 sec.

With bag files this same trick can actually be applied to all transforms not just static ones. If TF messages are published from the bag 1 second before their actual transform stamp, then TF listeners will have the necessary data in their buffers to get the transform for sensor messages.

Here is a script we often use to re-order bag files so that playback is more well behaved. It re-orders the messages so that they are ordered and published relative to their header stamp instead of serialization to disk stamp and TF data is ordered 1 second earlier. It also falls back to the original bag stamp if the difference is larger than a configurable duration, which mainly handles cases where an incorrect stamp like 0 was given to a message in the first place:

#!/usr/bin/env python

import sys
import rosbag
import time
import subprocess
import yaml
import rospy
import os
import argparse
import math
from shutil import move

def status(length, percent):
  #sys.stdout.write("%3d%%\r" % percent)
  sys.stdout.write('\x1B[2K') # Erase entire current line
  sys.stdout.write('\x1B[0E') # Move to the beginning of the current line
  progress = "Progress: ["
  for i in range(0, length):
    if i < length * percent:
      progress += '='
    else:
      progress += ' '
  progress += "] " + str(round(percent * 100.0, 2)) + "%"
  sys.stdout.write(progress)
  sys.stdout.flush()

def main(args):

  parser = argparse.ArgumentParser(description='Reorder a bagfile based on header timestamps.')
  parser.add_argument('bagfile', nargs=1, help='input bag file')
  parser.add_argument('--max-offset', nargs=1, help='max time offset (sec) to correct.', default='60', type=float)
  args = parser.parse_args()

  # Get bag duration  

  bagfile = args.bagfile[0]

  info_dict = yaml.load(subprocess.Popen(['rosbag', 'info', '--yaml', bagfile], stdout=subprocess.PIPE).communicate()[0])
  duration = info_dict['duration']
  start_time = info_dict['start']

  orig = os.path.splitext(bagfile)[0] + ".orig.bag"

  move(bagfile, orig)

  with rosbag.Bag(bagfile, 'w') as outbag:

    last_time = time.clock()
    for topic, msg, t in rosbag.Bag(orig).read_messages():

      if time.clock() - last_time > .1:
          percent = (t.to_sec() - start_time) / duration
          status(40, percent)
          last_time = time.clock()

      if topic == "/tf" and msg.transforms:
        # Writing transforms to bag file 1 second ahead of time to ensure availability
        diff = math.fabs(msg.transforms[0].header.stamp.to_sec() - t.to_sec())
        outbag.write(topic, msg, msg.transforms[0].header.stamp - rospy.Duration(1) if diff < args.max_offset else t)
      elif msg._has_header:
        diff = math.fabs(msg.header.stamp.to_sec() - t.to_sec())
        outbag.write(topic, msg, msg.header.stamp if diff < args.max_offset else t)
      else:
        outbag.write(topic, msg, t)
  status(40, 1)
  print "\ndone"

if __name__ == "__main__":
  main(sys.argv[1:])

[facontidavide]

thank you very much for the advice :+1: