From b5bc1d6f20630fd98ab424c91fd3795e7f4fe77a Mon Sep 17 00:00:00 2001 From: "Hunter L. Allen" Date: Wed, 10 Mar 2021 10:22:25 -0500 Subject: [PATCH 1/4] Start implementing new argument parsing Signed-off-by: Hunter L. Allen --- .../static_transform_broadcaster_program.cpp | 184 ++++++++++++++---- 1 file changed, 144 insertions(+), 40 deletions(-) diff --git a/tf2_ros/src/static_transform_broadcaster_program.cpp b/tf2_ros/src/static_transform_broadcaster_program.cpp index 691ef0c44..375e96c9f 100644 --- a/tf2_ros/src/static_transform_broadcaster_program.cpp +++ b/tf2_ros/src/static_transform_broadcaster_program.cpp @@ -35,60 +35,160 @@ #include #include #include +#include +#include #include +std::unordered_map _make_arg_map(std::vector && args) +{ + std::unordered_map ret; + /* collect from [exe] --option1 value --option2 value ... --optionN value */ + for (size_t x = 1; x < args.size(); x += 2) { + ret.emplace(std::move(args[x]), std::move(args[x + 1])); + } + return ret; +} + +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 as with roll, pitch, yaw euler angles" + " (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 a vector represention the translation\n" + " --y Y y component of a vector represention of the translation\n" + " --z Z z component of a vector represention of the translation\n" + " --qx QX x component of a quaternion represention of the rotation\n" + " --qy QY y component of a quaternion representation of the rotation\n" + " --qz QZ z component of a quaternion representation of the rotation\n" + " --qw QW w component of a quaternion representation of the rotation\n" + " --roll ROLL roll component of an Euler representation of the rotation (RPY)\n" + " --pitch PITCH pitch component of an Euler representation of the rotation (RPY)\n" + " --yaw YAW yaw component of an Euler representation of the rotation (RPY)"; + printf("%s\n", usage); +} + +tf2::Quaternion _get_rotation(const std::unordered_map & args) +{ + tf2::Quaternion quat; + bool found_quaternion = false; + bool found_rpy = false; + auto iter = args.find("--qx"); + if (iter != args.end()) { + quat.setX(std::stod(iter->second)); + found_quaternion = true; + } + iter = args.find("--qy"); + if (iter != args.end()) { + quat.setY(std::stod(iter->second)); + found_quaternion = true; + } + iter = args.find("--qz"); + if (iter != args.end()) { + quat.setZ(std::stod(iter->second)); + found_quaternion = true; + } + iter = args.find("--qw"); + if (iter != args.end()) { + quat.setW(std::stod(iter->second)); + found_quaternion = true; + } + /* otherwise, look for roll, pitch, yaw */ + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + iter = args.find("--roll"); + if (iter != args.end()) { + roll = std::stod(iter->second); + found_rpy = true; + } + iter = args.find("--pitch"); + if (iter != args.end()) { + pitch = std::stod(iter->second); + found_rpy = true; + } + iter = args.find("--yaw"); + if (iter != args.end()) { + yaw = std::stod(iter->second); + found_rpy = true; + } + if (found_quaternion && found_rpy) { + RCUTILS_LOG_ERROR("cannot mix euler and quaternion arguments"); + _print_usage(); + exit(1); + } else if (!found_quaternion) { + quat.setRPY(roll, pitch, yaw); + } + return quat; +} + +tf2::Vector3 _get_translation(const std::unordered_map & args) +{ + tf2::Vector3 trans; + auto iter = args.find("--x"); + if (iter != args.end()) { + trans.setX(std::stod(iter->second)); + } + iter = args.find("--y"); + if (iter != args.end()) { + trans.setY(std::stod(iter->second)); + } + iter = args.find("--z"); + if (iter != args.end()) { + trans.setZ(std::stod(iter->second)); + } + return trans; +} + int main(int argc, char ** argv) { // Initialize ROS std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); + std::unordered_map arg_map = _make_arg_map(std::move(args)); rclcpp::NodeOptions options; std::shared_ptr node; - - 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; + tf2::Quaternion rotation; + tf2::Vector3 translation; + try { + rotation = _get_rotation(arg_map); + translation = _get_translation(arg_map); + } catch (std::invalid_argument & e) { + RCUTILS_LOG_ERROR("error parsing command line arguments"); + _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]; + auto iter = arg_map.find("--frame-id"); + if (iter == arg_map.end()) { + _print_usage(); + return 1; + } + frame_id = iter->second; + iter = arg_map.find("--child-frame-id"); + if (iter == arg_map.end()) { + _print_usage(); + return 1; } + child_id = iter->second; // 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}, }); @@ -96,7 +196,11 @@ int main(int argc, char ** argv) node = std::make_shared(options); RCLCPP_INFO( - node->get_logger(), "Spinning until killed publishing transform from '%s' to '%s'", + node->get_logger(), + "Spinning until killed 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_id.c_str()); rclcpp::spin(node); return 0; From d8f79c41c7d8525749427a4bd39555bea3fa31ea Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Sep 2021 15:39:33 +0000 Subject: [PATCH 2/4] 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: Chris Lalancette --- .../static_transform_broadcaster_program.cpp | 431 +++++++++++++----- 1 file changed, 317 insertions(+), 114 deletions(-) diff --git a/tf2_ros/src/static_transform_broadcaster_program.cpp b/tf2_ros/src/static_transform_broadcaster_program.cpp index 375e96c9f..7a36770a1 100644 --- a/tf2_ros/src/static_transform_broadcaster_program.cpp +++ b/tf2_ros/src/static_transform_broadcaster_program.cpp @@ -28,157 +28,354 @@ */ #include +#include #include #include #include +#include #include +#include #include #include -#include #include -std::unordered_map _make_arg_map(std::vector && args) +struct Option { - std::unordered_map ret; - /* collect from [exe] --option1 value --option2 value ... --optionN value */ - for (size_t x = 1; x < args.size(); x += 2) { - ret.emplace(std::move(args[x]), std::move(args[x + 1])); + explicit Option(bool has_arg) + : has_argument(has_arg) + { } - return ret; -} -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 as with roll, pitch, yaw euler angles" - " (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 a vector represention the translation\n" - " --y Y y component of a vector represention of the translation\n" - " --z Z z component of a vector represention of the translation\n" - " --qx QX x component of a quaternion represention of the rotation\n" - " --qy QY y component of a quaternion representation of the rotation\n" - " --qz QZ z component of a quaternion representation of the rotation\n" - " --qw QW w component of a quaternion representation of the rotation\n" - " --roll ROLL roll component of an Euler representation of the rotation (RPY)\n" - " --pitch PITCH pitch component of an Euler representation of the rotation (RPY)\n" - " --yaw YAW yaw component of an Euler representation of the rotation (RPY)"; - printf("%s\n", usage); -} + bool has_argument{false}; + + virtual std::string visit(const std::string & optname, const std::string & stringval) = 0; -tf2::Quaternion _get_rotation(const std::unordered_map & args) + virtual ~Option() {} +}; + +struct DoubleOption final : Option { - tf2::Quaternion quat; - bool found_quaternion = false; - bool found_rpy = false; - auto iter = args.find("--qx"); - if (iter != args.end()) { - quat.setX(std::stod(iter->second)); - found_quaternion = true; + explicit DoubleOption(bool has_arg, std::function cb) + : Option(has_arg), + callback(cb) + { + } + + std::function 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 ""; } - iter = args.find("--qy"); - if (iter != args.end()) { - quat.setY(std::stod(iter->second)); - found_quaternion = true; +}; + +struct StringOption final : Option +{ + explicit StringOption(bool has_arg, std::function cb) + : Option(has_arg), + callback(cb) + { } - iter = args.find("--qz"); - if (iter != args.end()) { - quat.setZ(std::stod(iter->second)); - found_quaternion = true; + + std::function callback; + + std::string visit(const std::string & optname, const std::string & stringval) override + { + (void)optname; + callback(stringval); + return ""; } - iter = args.find("--qw"); - if (iter != args.end()) { - quat.setW(std::stod(iter->second)); - found_quaternion = true; +}; + +static std::string parse_args( + const std::vector & args, + bool & help, + tf2::Quaternion & quat, + tf2::Vector3 & trans, + std::string & frame_id, + std::string & child_frame_id) +{ + size_t size = args.size(); + size_t last_index = size - 1; + + if (size < 1) { + return "Not enough arguments to parse"; } - /* otherwise, look for roll, pitch, yaw */ + + help = false; + + bool saw_quat_flag = false; + bool saw_rpy_flag = false; double roll = 0.0; double pitch = 0.0; double yaw = 0.0; - iter = args.find("--roll"); - if (iter != args.end()) { - roll = std::stod(iter->second); - found_rpy = true; - } - iter = args.find("--pitch"); - if (iter != args.end()) { - pitch = std::stod(iter->second); - found_rpy = true; + + bool saw_trans_flag = false; + + bool saw_frame_flag = false; + + auto qx_opt = std::make_shared( + true, [&quat, &saw_quat_flag](double value) { + quat.setX(value); + saw_quat_flag = true; + }); + + auto qy_opt = std::make_shared( + true, [&quat, &saw_quat_flag](double value) { + quat.setY(value); + saw_quat_flag = true; + }); + + auto qz_opt = std::make_shared( + true, [&quat, &saw_quat_flag](double value) { + quat.setZ(value); + saw_quat_flag = true; + }); + + auto qw_opt = std::make_shared( + true, [&quat, &saw_quat_flag](double value) { + quat.setW(value); + saw_quat_flag = true; + }); + + auto roll_opt = std::make_shared( + true, [&roll, &saw_rpy_flag](double value) { + roll = value; + saw_rpy_flag = true; + }); + + auto pitch_opt = std::make_shared( + true, [&pitch, &saw_rpy_flag](double value) { + pitch = value; + saw_rpy_flag = true; + }); + + auto yaw_opt = std::make_shared( + true, [&yaw, &saw_rpy_flag](double value) { + yaw = value; + saw_rpy_flag = true; + }); + + auto trans_x_opt = std::make_shared( + true, [&trans, &saw_trans_flag](double value) { + trans.setX(value); + saw_trans_flag = true; + }); + + auto trans_y_opt = std::make_shared( + true, [&trans, &saw_trans_flag](double value) { + trans.setY(value); + saw_trans_flag = true; + }); + + auto trans_z_opt = std::make_shared( + true, [&trans, &saw_trans_flag](double value) { + trans.setZ(value); + saw_trans_flag = true; + }); + + auto frame_id_opt = std::make_shared( + 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( + 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( + false, [&help](const std::string & value) { + (void)value; + help = true; + }); + + std::unordered_map> 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 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