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 +#include #include #include #include +#include #include +#include #include +#include #include +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 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 ""; + } +}; + +struct StringOption final : Option +{ + explicit StringOption(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 + { + (void)optname; + callback(stringval); + return ""; + } +}; + +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(); + + 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( + 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