swri-robotics / marti_common

Common utility functions for MARTI
BSD 3-Clause "New" or "Revised" License
54 stars 63 forks source link

swri_transform_util's initialize_origin.py incorrectly handles parameter #747

Closed DangitBen closed 1 month ago

DangitBen commented 1 month ago

The local_xy_origins parameter of initialize_origin.py is manually set to type rclpy.parameter.ParameterType.PARAMETER_DOUBLE_ARRAY but the documentation (and usage) expects it to also be able to accept a rclpy.parameter.ParameterType.PARAMETER_STRING. Specifying this type means that rclpy will error out instead of accepting both types flexibly.

This means that uses like mapviz's launch cannot specify a yaml string with named origins (also making local_xy_origin values other than auto useless). Additionally, this code path to process the yaml string cannot be reached.

danthony06 commented 1 month ago

I talked this over with @rjb0026, and we're thinking that we can turn that parameter into a string array, where the origin name is specified, and then the lat, lon, altitude, and heading of the origin are specified. Since ROS 2 doesn't support passing in a dictionary type, this seems like it may be the best way.

rjb0026 commented 1 month ago

@DangitBen if you have a different solution suggestion and want to throw up a PR, I'll be happy to jump on a review for it and test it out.

DangitBen commented 1 month ago

I think all you would have to do to restore the previous functionality is just omit the type specification. It already has logic to handle the dynamic typing to figure out if it is the double array or a string. The string approach didn't rely on ROS 1's dictionary support. It just loads a yaml string, which is not unlike other tools (e.g., robot_state_publisher). This lets you load content either from a separate config file manually or specify it directly as a parameter or argument.

If you want to take this opportunity to change how the parameter works and break backwards compatibility, then that's fine by me. The string array is probably reasonable, but it could suffer from the dreaded LLA ordering issue (["name1 lat1 lon1 alt1 heading1"] or ["name2 lon2 lat2 alt2 heading2"]).

I guess this could be solved (and maybe it's what you were suggesting) by making each string in the array a yaml dict.

parameters=[{
    "local_xy_origin": "origin1",
    "local_xy_origins": ["{name: 'origin1', latitude: -90.0, longitude: 45.0, altitude: 1.0, heading: -90.0}", ""}]
}]
DangitBen commented 1 month ago

It ended up being a little more involved of a change. A bit of the logic in there was really not valid anymore. See the potential PR