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