Allow to choose output precision in echo

[VictorLamoine]

Similar to ros/geometry#186

With this PR it is possible to choose the precision of the pose output by passing an argument to the node:

rosrun tf2_tools echo.py base tool0 -p 8

I am inspecting robot poses and I need at least millimeter precision. This allows getting the precision without cluttering the default output.

Example run:

rosrun tf2_tools echo.py base tool0 -p 8
At time 1556183292.91, (current time 1556183292.96)
- Translation: [1.34024981, 0.00000000, 1.04058951]
- Rotation: in Quaternion [0.70693168, 0.00000000, 0.70728184, -0.00000000]
            in RPY (radian) [0.00000000, -1.57030113, 3.14159265]
            in RPY (degree) [0.00000000, -89.97162713, 180.00000000]

[tfoote]

Thanks this looks good. For symmetry to the other implementation could you add support for reading the ros parameter with second priority behind the command line option? That way if a user switches their launch file usage can stay the same.

[VictorLamoine]

I don't know how to use both argparse and ROS parameters. Can you give me a hand?

if __name__ == '__main__':
    rospy.init_node("echo")

    parser = argparse.ArgumentParser()
    parser.add_argument("source_frame")  # parent
    parser.add_argument("target_frame")  # child
    parser.add_argument("-r", "--rate",
                        help="update rate, must be > 0.0",
                        default=1.0,
                        type=positive_float)
    parser.add_argument("-c", "--cache_time",
                        help="length of tf buffer cache in seconds",
                        type=positive_float)
    parser.add_argument("-o", "--offset",
                        help="offset the lookup from current time, ignored if using -t",
                        type=float)
    parser.add_argument("-t", "--time",
                        help="fixed time to do the lookup",
                        type=float)
    parser.add_argument("-l", "--limit",
                        help="lookup fixed number of times",
                        type=positive_int)
    parser.add_argument("-p", "--precision",
                        help="output precision",
                        default=3,
                        type=positive_int)
    args = parser.parse_args()
    try:
        precision = rospy.get_param('~precision')
        rospy.loginfo(precision)
        # FIXME Change args
    except KeyError:
        pass
    echo = Echo(args)
    rospy.spin()

rosrun tf2_tools echo.py base tool0 _precision:=12
usage: echo.py [-h] [-r RATE] [-c CACHE_TIME] [-o OFFSET] [-t TIME] [-l LIMIT]
               [-p PRECISION]
               source_frame target_frame
echo.py: error: unrecognized arguments: _precision:=12

I have also tried args = parser.parse_known_args() it helps but I don't understand what else is wrong:

[INFO] [1556527133.085008]: 12
Traceback (most recent call last):
  File "/home/victor/catkin/src/geometry2/tf2_tools/scripts/echo.py", line 246, in <module>
    echo = Echo(args)
  File "/home/victor/catkin/src/geometry2/tf2_tools/scripts/echo.py", line 142, in __init__
    self.tf_buffer = tf2_ros.Buffer(cache_time=args.cache_time)
AttributeError: 'tuple' object has no attribute 'cache_time'

[tfoote]

There's a helper function to strip the ROS arguments from the command line:

http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown#Accessing_your_command-line_arguments

Then pass myargv to parse_args

Something like:

myargv = rospy.myargv(argv=sys.argv)
args = parser.parse_args(myargv)

[VictorLamoine]

Thanks for the help! Now you can also use the ROS parameter to set the precision.
Tell me if there's anything you would like to see change

[tfoote]

Looks good to me. Thanks for the improvement here too!