From 254748f2e94dc560104f2b74482e05bacaf28132 Mon Sep 17 00:00:00 2001
From: "Hunter L. Allen" <hunterlallen@protonmail.com>
Date: Wed, 13 Oct 2021 07:13:02 -0500
Subject: [PATCH] More Intuitive CLI for Static Transform Publisher (#392)

* Rewrite the command-line handling to use flags.

In this approach, we still allow the old command-line style,
but it prints a warning and is deprecated.  The new style is
to use flags (like --x, --qx, etc), so that it is clear to
the user which axes they are overriding.

Signed-off-by: Hunter L. Allen <hunter.allen@ghostrobotics.io>
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
---
 test_tf2/test/buffer_client_tester.launch.py  |   2 +-
 test_tf2/test/static_publisher.launch.py      |   4 +-
 test_tf2/test/test_buffer_client.launch.py    |   2 +-
 .../static_transform_broadcaster_program.cpp  | 394 ++++++++++++++++--
 4 files changed, 354 insertions(+), 48 deletions(-)

diff --git a/test_tf2/test/buffer_client_tester.launch.py b/test_tf2/test/buffer_client_tester.launch.py
index 2632a5e41..5cd426c55 100644
--- a/test_tf2/test/buffer_client_tester.launch.py
+++ b/test_tf2/test/buffer_client_tester.launch.py
@@ -22,7 +22,7 @@ def generate_test_description():
         package='tf2_ros',
         executable='static_transform_publisher',
         output='screen',
-        arguments=["5", "6", "7", "0", "0", "0", "1", "a", "b"]
+        arguments=["--x", "5", "--y", "6", "--z", "7", "--qx", "0", "--qy", "0", "--qz", "0", "--qw", "1", "--frame-id", "a", "--child-frame-id", "b"]
     )
 
     node_buffer_server = Node(
diff --git a/test_tf2/test/static_publisher.launch.py b/test_tf2/test/static_publisher.launch.py
index c2ae21e2a..a995fa0ce 100644
--- a/test_tf2/test/static_publisher.launch.py
+++ b/test_tf2/test/static_publisher.launch.py
@@ -21,13 +21,13 @@ def generate_test_description():
         package='tf2_ros',
         executable='static_transform_publisher',
         output='screen',
-        arguments=["1", "0", "0", "0", "0", "0", "1", "a", "b"]
+        arguments=["--x", "1", "--y", "0", "--z", "0", "--qx", "0", "--qy", "0", "--qz", "0", "--qw", "1", "--frame-id", "a", "--child-frame-id", "b"]
     )
     node_static_transform_publisher_2 = Node(
         package='tf2_ros',
         executable='static_transform_publisher',
         output='screen',
-        arguments=["0", "1", "0", "0", "0", "0", "1", "b", "c"]
+        arguments=["--x", "0", "--y", "1", "--z", "0", "--qx", "0", "--qy", "0", "--qz", "0", "--qw", "1", "--frame-id", "b", "--child-frame-id", "c"]
     )
 
     return LaunchDescription([
diff --git a/test_tf2/test/test_buffer_client.launch.py b/test_tf2/test/test_buffer_client.launch.py
index 3a2b82dc5..edfa968af 100644
--- a/test_tf2/test/test_buffer_client.launch.py
+++ b/test_tf2/test/test_buffer_client.launch.py
@@ -21,7 +21,7 @@ def generate_test_description():
         package='tf2_ros',
         executable='static_transform_publisher',
         output='screen',
-        arguments=["5", "6", "7", "0", "0", "0", "1", "a", "b"]
+        arguments=["--x", "5", "--y", "6", "--z", "7", "--qx", "0", "--qy", "0", "--qz", "0", "--qw", "1", "--frame-id", "a", "--child-frame-id", "b"]
     )
 
     node_buffer_server = Node(
diff --git a/tf2_ros/src/static_transform_broadcaster_program.cpp b/tf2_ros/src/static_transform_broadcaster_program.cpp
index 691ef0c44..e3f523686 100644
--- a/tf2_ros/src/static_transform_broadcaster_program.cpp
+++ b/tf2_ros/src/static_transform_broadcaster_program.cpp
@@ -28,76 +28,382 @@
  */
 
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
 
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_ros/static_transform_broadcaster_node.hpp>
 
 #include <cstdio>
+#include <functional>
 #include <memory>
+#include <stdexcept>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
+struct Option
+{
+  explicit Option(bool has_arg)
+  : has_argument(has_arg)
+  {
+  }
+
+  bool has_argument{false};
+
+  virtual std::string visit(const std::string & optname, const std::string & stringval) = 0;
+
+  virtual ~Option() {}
+};
+
+struct DoubleOption final : Option
+{
+  explicit DoubleOption(bool has_arg, std::function<void(double)> cb)
+  : Option(has_arg),
+    callback(cb)
+  {
+  }
+
+  std::function<void(double)> callback;
+
+  std::string visit(const std::string & optname, const std::string & stringval) override
+  {
+    double value;
+    try {
+      value = std::stod(stringval);
+    } catch (const std::invalid_argument &) {
+      return "Failed to parse " + optname + " argument as float";
+    }
+
+    callback(value);
+
+    return "";
+  }
+};
+
+struct StringOption final : Option
+{
+  explicit StringOption(bool has_arg, std::function<void(const std::string &)> cb)
+  : Option(has_arg),
+    callback(cb)
+  {
+  }
+
+  std::function<void(const std::string &)> callback;
+
+  std::string visit(const std::string & optname, const std::string & stringval) override
+  {
+    (void)optname;
+    callback(stringval);
+    return "";
+  }
+};
+
+static std::string parse_args(
+  const std::vector<std::string> & args,
+  bool & help,
+  tf2::Quaternion & quat,
+  tf2::Vector3 & trans,
+  std::string & frame_id,
+  std::string & child_frame_id)
+{
+  size_t size = args.size();
+
+  help = false;
+
+  if (size < 1) {
+    return "Not enough arguments to parse";
+  }
+
+  size_t last_index = size - 1;
+
+  bool saw_frame_flag = false;
+  bool saw_quat_flag = false;
+  bool saw_rpy_flag = false;
+  bool saw_trans_flag = false;
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+
+  auto qx_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setX(value);
+      saw_quat_flag = true;
+    });
+
+  auto qy_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setY(value);
+      saw_quat_flag = true;
+    });
+
+  auto qz_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setZ(value);
+      saw_quat_flag = true;
+    });
+
+  auto qw_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setW(value);
+      saw_quat_flag = true;
+    });
+
+  auto roll_opt = std::make_shared<DoubleOption>(
+    true, [&roll, &saw_rpy_flag](double value) {
+      roll = value;
+      saw_rpy_flag = true;
+    });
+
+  auto pitch_opt = std::make_shared<DoubleOption>(
+    true, [&pitch, &saw_rpy_flag](double value) {
+      pitch = value;
+      saw_rpy_flag = true;
+    });
+
+  auto yaw_opt = std::make_shared<DoubleOption>(
+    true, [&yaw, &saw_rpy_flag](double value) {
+      yaw = value;
+      saw_rpy_flag = true;
+    });
+
+  auto trans_x_opt = std::make_shared<DoubleOption>(
+    true, [&trans, &saw_trans_flag](double value) {
+      trans.setX(value);
+      saw_trans_flag = true;
+    });
+
+  auto trans_y_opt = std::make_shared<DoubleOption>(
+    true, [&trans, &saw_trans_flag](double value) {
+      trans.setY(value);
+      saw_trans_flag = true;
+    });
+
+  auto trans_z_opt = std::make_shared<DoubleOption>(
+    true, [&trans, &saw_trans_flag](double value) {
+      trans.setZ(value);
+      saw_trans_flag = true;
+    });
+
+  auto frame_id_opt = std::make_shared<StringOption>(
+    true, [&frame_id, &saw_frame_flag](const std::string & value) {
+      frame_id = value;
+      saw_frame_flag = true;
+    });
+
+  auto child_frame_id_opt = std::make_shared<StringOption>(
+    true, [&child_frame_id, &saw_frame_flag](const std::string & value) {
+      child_frame_id = value;
+      saw_frame_flag = true;
+    });
+
+  auto help_opt = std::make_shared<StringOption>(
+    false, [&help](const std::string & value) {
+      (void)value;
+      help = true;
+    });
+
+  std::unordered_map<std::string, std::shared_ptr<Option>> options = {
+    {"--qx", qx_opt},
+    {"--qy", qy_opt},
+    {"--qz", qz_opt},
+    {"--qw", qw_opt},
+    {"--roll", roll_opt},
+    {"--pitch", pitch_opt},
+    {"--yaw", yaw_opt},
+    {"--x", trans_x_opt},
+    {"--y", trans_y_opt},
+    {"--z", trans_z_opt},
+    {"--frame-id", frame_id_opt},
+    {"--child-frame-id", child_frame_id_opt},
+    {"--help", help_opt},
+    {"-h", help_opt},
+  };
+
+  std::vector<std::string> no_flag_args;
+
+  size_t i = 1;
+  while (i < size) {
+    const std::string & optname = args[i];
+    if (options.count(optname) == 0) {
+      no_flag_args.push_back(optname);
+    } else {
+      std::shared_ptr<Option> opt = options[optname];
+      if (opt->has_argument) {
+        if (i == last_index) {
+          return "Not enough arguments for " + optname;
+        }
+
+        ++i;
+      }
+      std::string result = opt->visit(optname, args[i]);
+      if (result != "") {
+        return result;
+      }
+    }
+    ++i;
+  }
+
+  if (help) {
+    return "";
+  }
+
+  if (saw_rpy_flag && saw_quat_flag) {
+    return "Cannot specify both quaternion and Euler rotations";
+  } else if (saw_rpy_flag) {
+    quat.setRPY(roll, pitch, yaw);
+  }
+
+  if (no_flag_args.size() == 8 || no_flag_args.size() == 9) {
+    RCUTILS_LOG_WARN("Old-style arguments are deprecated; see --help for new-style arguments");
+    if (saw_frame_flag || saw_trans_flag || saw_quat_flag) {
+      return "Cannot specify both new-style (flags) and old-style (arguments)";
+    }
+
+    std::string ret;
+
+    ret = trans_x_opt->visit("x", no_flag_args[0]);
+    if (ret != "") {
+      return ret;
+    }
+    ret = trans_y_opt->visit("y", no_flag_args[1]);
+    if (ret != "") {
+      return ret;
+    }
+    ret = trans_z_opt->visit("z", no_flag_args[2]);
+    if (ret != "") {
+      return ret;
+    }
+
+    if (no_flag_args.size() == 8) {
+      ret = yaw_opt->visit("yaw", no_flag_args[3]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = pitch_opt->visit("pitch", no_flag_args[4]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = roll_opt->visit("roll", no_flag_args[5]);
+      if (ret != "") {
+        return ret;
+      }
+
+      quat.setRPY(roll, pitch, yaw);
+      frame_id = no_flag_args[6];
+      child_frame_id = no_flag_args[7];
+    } else {
+      ret = qx_opt->visit("qx", no_flag_args[3]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = qy_opt->visit("qy", no_flag_args[4]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = qz_opt->visit("qz", no_flag_args[5]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = qw_opt->visit("qw", no_flag_args[6]);
+      if (ret != "") {
+        return ret;
+      }
+      frame_id = no_flag_args[7];
+      child_frame_id = no_flag_args[8];
+    }
+  } else if (no_flag_args.size() != 0) {
+    return "Extra unparsed arguments on command-line";
+  }
+
+  if (frame_id == "") {
+    return "Frame id must not be empty";
+  }
+
+  if (child_frame_id == "") {
+    return "Child frame id must not be empty";
+  }
+
+  return "";
+}
+
+static void print_usage()
+{
+  const char * usage =
+    "usage: static_transform_publisher [--x X] [--y Y] [--z Z] [--qx QX] [--qy QY] "
+    "[--qz QZ] [--qw QW] [--roll ROLL] [--pitch PITCH] [--yaw YAW] --frame-id FRAME_ID "
+    "--child-frame-id CHILD_FRAME_ID\n\n"
+    "A command line utility for manually sending a static transform.\n\nIf no translation or"
+    " orientation is provided, the identity transform will be published.\n\nThe translation offsets"
+    " are in meters.\n\nThe rotation may be provided with roll, pitch, yaw euler angles in radians,"
+    " or as a quaternion.\n\n"
+    "required arguments:\n"
+    "  --frame-id FRAME_ID parent frame\n"
+    "  --child-frame-id CHILD_FRAME_ID child frame id\n\n"
+    "optional arguments:\n"
+    "  --x X                 x component of translation\n"
+    "  --y Y                 y component of translation\n"
+    "  --z Z                 z component of translation\n"
+    "  --qx QX               x component of quaternion rotation\n"
+    "  --qy QY               y component of quaternion rotation\n"
+    "  --qz QZ               z component of quaternion rotation\n"
+    "  --qw QW               w component of quaternion rotation\n"
+    "  --roll ROLL           roll component Euler rotation\n"
+    "  --pitch PITCH         pitch component Euler rotation\n"
+    "  --yaw YAW             yaw component Euler rotation";
+  printf("%s\n", usage);
+}
+
 int main(int argc, char ** argv)
 {
   // Initialize ROS
   std::vector<std::string> args = rclcpp::init_and_remove_ros_arguments(argc, argv);
-  rclcpp::NodeOptions options;
-  std::shared_ptr<tf2_ros::StaticTransformBroadcasterNode> node;
+  bool help = false;
+  tf2::Quaternion rotation(0.0, 0.0, 0.0, 1.0);
+  tf2::Vector3 translation(0.0, 0.0, 0.0);
+  std::string frame_id;
+  std::string child_frame_id;
 
-  if (args.size() != 9 && args.size() != 10) {
-    printf("A command line utility for manually sending a transform.\n");
-    printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n");
-    printf("OR \n");
-    printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n");
-    RCUTILS_LOG_ERROR(
-      "static_transform_publisher exited due to not having the right number of arguments");
-    return 2;
+  std::string ret = parse_args(args, help, rotation, translation, frame_id, child_frame_id);
+  if (ret != "") {
+    RCUTILS_LOG_ERROR("error parsing command line arguments: %s", ret.c_str());
+    print_usage();
+    return 1;
   }
-  double x = std::stod(args[1]);
-  double y = std::stod(args[2]);
-  double z = std::stod(args[3]);
-  double rx, ry, rz, rw;
-  std::string frame_id, child_id;
-
-  if (args.size() == 9) {
-    // grab parameters from yaw, pitch, roll
-    tf2::Quaternion quat;
-    quat.setRPY(std::stod(args[6]), std::stod(args[5]), std::stod(args[4]));
-    rx = quat.x();
-    ry = quat.y();
-    rz = quat.z();
-    rw = quat.w();
-    frame_id = args[7];
-    child_id = args[8];
-  } else {
-    // quaternion supplied directly
-    rx = std::stod(args[4]);
-    ry = std::stod(args[5]);
-    rz = std::stod(args[6]);
-    rw = std::stod(args[7]);
-    frame_id = args[8];
-    child_id = args[9];
+  if (help) {
+    print_usage();
+    return 0;
   }
 
+  rclcpp::NodeOptions options;
   // override default parameters with the desired transform
   options.parameter_overrides(
   {
-    {"translation.x", x},
-    {"translation.y", y},
-    {"translation.z", z},
-    {"rotation.x", rx},
-    {"rotation.y", ry},
-    {"rotation.z", rz},
-    {"rotation.w", rw},
+    {"translation.x", translation.x()},
+    {"translation.y", translation.y()},
+    {"translation.z", translation.z()},
+    {"rotation.x", rotation.x()},
+    {"rotation.y", rotation.y()},
+    {"rotation.z", rotation.z()},
+    {"rotation.w", rotation.w()},
     {"frame_id", frame_id},
-    {"child_frame_id", child_id},
+    {"child_frame_id", child_frame_id},
   });
 
+  std::shared_ptr<tf2_ros::StaticTransformBroadcasterNode> node;
+
   node = std::make_shared<tf2_ros::StaticTransformBroadcasterNode>(options);
 
   RCLCPP_INFO(
-    node->get_logger(), "Spinning until killed publishing transform from '%s' to '%s'",
-    frame_id.c_str(), child_id.c_str());
+    node->get_logger(),
+    "Spinning until stopped - publishing transform\ntranslation: ('%lf', '%lf', '%lf')\n"
+    "rotation: ('%lf', '%lf', '%lf', '%lf')\nfrom '%s' to '%s'",
+    translation.x(), translation.y(), translation.z(),
+    rotation.x(), rotation.y(), rotation.z(), rotation.w(),
+    frame_id.c_str(), child_frame_id.c_str());
+
   rclcpp::spin(node);
+
+  rclcpp::shutdown();
+
   return 0;
 }