Skip to content

Commit

Permalink
Update origin params (#752)
Browse files Browse the repository at this point in the history
* Update origin publisher params

---------

Co-authored-by: Ben <benjamin.andrew@swri.org>
  • Loading branch information
DangitBen and Ben authored Sep 18, 2024
1 parent 429a257 commit 6e7f080
Show file tree
Hide file tree
Showing 4 changed files with 39 additions and 21 deletions.
41 changes: 23 additions & 18 deletions swri_transform_util/nodes/initialize_origin.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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)
Expand All @@ -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):
Expand Down
1 change: 1 addition & 0 deletions swri_transform_util/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
2 changes: 1 addition & 1 deletion swri_transform_util/swri_transform_util/origin_manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
16 changes: 14 additions & 2 deletions swri_transform_util/test/initialize_origin_manual.test.py
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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(),
]
Expand Down

0 comments on commit 6e7f080

Please sign in to comment.