diff --git a/swri_transform_util/nodes/initialize_origin.py b/swri_transform_util/nodes/initialize_origin.py index 7d739d622..dbe3dc05d 100755 --- a/swri_transform_util/nodes/initialize_origin.py +++ b/swri_transform_util/nodes/initialize_origin.py @@ -36,6 +36,7 @@ from sensor_msgs.msg import NavSatFix from swri_transform_util.origin_manager import OriginManager, InvalidFixException import sys +import yaml class OriginInitializer(rclpy.node.Node): @@ -68,10 +69,10 @@ def __init__(self): type=rclpy.parameter.ParameterType.PARAMETER_STRING )) self.local_xy_origins_param = self.declare_parameter('local_xy_origins', - [nan, nan, nan], + [nan, nan, nan, nan], descriptor=rclpy.node.ParameterDescriptor( name='local_xy_origins', - type=rclpy.parameter.ParameterType.PARAMETER_DOUBLE_ARRAY + dynamic_typing=True )) self.get_logger().info("Origin: %s" % self.local_xy_origin_param.value) @@ -90,25 +91,29 @@ def __init__(self): local_xy_navsatfix_topic, self.navsat_callback, 2) self.subscribers = [gps_sub, navsat_sub] - else: - try: - self.get_logger().info("local_xy_origins_param[0]: %lf" % self.local_xy_origins_param.value[0]) - self.get_logger().info("local_xy_origins_param[1]: %lf" % self.local_xy_origins_param.value[1]) - origin_dict={'latitude': self.local_xy_origins_param.value[0], - 'longitude':self.local_xy_origins_param.value[1], - 'altitude':self.local_xy_origins_param.value[2], - 'heading':self.local_xy_origins_param.value[3]} - except rclpy.exceptions.ParameterException: - message = 'local_xy_origin is "{}", but local_xy_origins is not specified' - self.get_logger().fatal(message.format(self.local_xy_origin_param.value)) - exit(1) + elif type(self.local_xy_origins_param.value) == list: + if (len(self.local_xy_origins_param.value) != 4): + self.get_logger().fatal(f'{self.local_xy_origins_param.name} should have len 4 [lat, lon, alt, heading]') + exit(1) + self.manager.set_origin("manual", *self.local_xy_origins_param.value[0:3]) + elif type(self.local_xy_origins_param.value) == str: try: - self.manager.set_origin_from_dict(origin_dict) + origins_list = yaml.safe_load(self.local_xy_origins_param.value) + self.manager.set_origin_from_list(self.local_xy_origin_param.value, origins_list) except (TypeError, KeyError) as e: - message = 'local_xy_origins is malformed or does not contain the local_xy_origin "{}"' - self.get_logger().fatal(message.format(self.local_xy_origin_param.value)) - self.get_logger().fatal("%s" % str(e)) + message = '{} is malformed or does not contain the {} "{}"' + self.get_logger().fatal(message.format(self.local_xy_origins_param.name, + self.local_xy_origin_param.name, + self.local_xy_origin_param.value)) + self.get_logger().fatal(str(e)) exit(1) + except (yaml.parser.ParserError) as e: + self.get_logger().fatal(f"'{self.local_xy_origins_param.name}' string value is malformed") + self.get_logger().fatal(f"yaml error: {str(e)}") + exit(1) + else: + self.get_logger().fatal(f"Parameter '{self.local_xy_origins_param.name}' has incorrect type. Expected: string or double array") + exit(1) self.manager.start() def navsat_callback(self, msg): diff --git a/swri_transform_util/package.xml b/swri_transform_util/package.xml index 63d483309..89002b661 100644 --- a/swri_transform_util/package.xml +++ b/swri_transform_util/package.xml @@ -42,6 +42,7 @@ yaml-cpp python3-numpy + python3-yaml ament_cmake_gtest ament_index_cpp diff --git a/swri_transform_util/swri_transform_util/origin_manager.py b/swri_transform_util/swri_transform_util/origin_manager.py index c6c7a5700..9e5185a5e 100644 --- a/swri_transform_util/swri_transform_util/origin_manager.py +++ b/swri_transform_util/swri_transform_util/origin_manager.py @@ -127,7 +127,7 @@ def __init__(self, node, local_xy_frame, local_xy_frame_identity=None): local_xy_frame_identity = local_xy_frame + "__identity" self.local_xy_frame_identity = local_xy_frame_identity qos = rclpy.node.QoSProfile(depth=1, - durability=rclpy.qos.QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL) self.origin_pub = node.create_publisher(PoseStamped, '/local_xy_origin', qos_profile=qos) self.diagnostic_pub = node.create_publisher(DiagnosticArray, '/diagnostics', 2) self.tf_broadcaster = TransformBroadcaster(self.node) diff --git a/swri_transform_util/test/initialize_origin_manual.test.py b/swri_transform_util/test/initialize_origin_manual.test.py index d24a11d99..0a60dc07a 100644 --- a/swri_transform_util/test/initialize_origin_manual.test.py +++ b/swri_transform_util/test/initialize_origin_manual.test.py @@ -56,7 +56,7 @@ def get_tests(*, args=[]): @pytest.mark.launch_test def generate_test_description(): - init_origin = Node( + init_origin_array = Node( package="swri_transform_util", name="origin", executable="initialize_origin.py", @@ -67,9 +67,21 @@ def generate_test_description(): }] ) + init_origin_dicitonary = Node( + package="swri_transform_util", + name="origin", + executable="initialize_origin.py", + parameters=[{ + "local_xy_frame": "/far_field", + "local_xy_origin": "swri", + "local_xy_origins": "[{name: 'swri', latitude: 29.45196669, longitude: -98.61370577, altitude: 233.719, heading: 0.0}]", + }] + ) + return launch.LaunchDescription( [ - init_origin, + init_origin_array, + init_origin_dicitonary, launch_testing.util.KeepAliveProc(), launch_testing.actions.ReadyToTest(), ]