Add substitution_args $(arg foo) support to xacro

This fixes #3 (https://code.ros.org/trac/ros-pkg/ticket/5420)