Update origin params

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):

swri_transform_util/package.xml

@@ -42,6 +42,7 @@
   <depend>yaml-cpp</depend>
 
   <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_index_cpp</test_depend>

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)

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(),
             ]