"""Adjusts Gazebos update rate to guarantee sensor updates with desired frequency."""
import contextlib
import rclpy
import yaml
from kitcar_utils.ros_base.node_base import NodeBase
from rclpy.wait_for_message import wait_for_message
from std_srvs.srv import Empty as EmptySrv
# TODO(gz): The ROS1 message types below were used together with rospy.AnyMsg to subscribe
# to arbitrary topics solely to measure their publishing frequency. ROS2 has no AnyMsg
# equivalent; the desired topics must be subscribed to with their concrete message types,
# or the frequency must be measured through gz-transport / ros2 topic hz. The target
# subscriptions are therefore disabled below and need sim verification.
[docs]
class GazeboRateControlNode(NodeBase):
"""Control gazebos update rate to meet desired sensor update rates.
Attributes:
relevant_targets (List[Dict[str, Any]]): Topics that should be monitored
and update rate that the topic should have.
_last_target_update (float): Last time the targets were updated.
_last_target_frequencies (List[float]): Last known update rate of each target.
"""
def __init__(self):
"""Initialize the node."""
super().__init__(name="gazebo_rate_control_node")
self._last_target_frequencies = {}
self._last_target_update = 0
self.relevant_targets = []
# The targets are a list of dicts and cannot be a ROS2 parameter.
self.targets = []
if self.param.targets_file:
with open(self.param.targets_file) as f:
self.targets = yaml.safe_load(f)["targets"]
super().run(function=self.update, rate=self.param.update_rate.control.rate)
[docs]
def start(self):
# TODO(gz): replace ROS1 /gazebo/get_physics_properties and
# /gazebo/set_physics_properties services with the gz-transport service
# `/world/<name>/set_physics` (and a corresponding state query). These ROS1
# services do not exist in gz-sim Harmonic. The physics clients are stubbed out
# here so the node still imports and runs; the actual rate control needs the
# running Gazebo Harmonic simulation to wire up.
self.set_physics = None
self.get_physics = None
if self.param.use_sync:
# TODO(gz): The sync source was subscribed to with rospy.AnyMsg; ROS2 needs the
# concrete message type of `sync.source_topic`. Subscription disabled until the
# source topic's type is known and the gz pause/unpause is wired up.
# TODO(gz): replace ROS1 /gazebo/pause_physics and /gazebo/unpause_physics with
# the gz-transport `/world/<name>/control` service (pause/step). Stubbed below.
self.pause_physics_proxy = self.create_client(
EmptySrv, self.param.topics.pause_gazebo
)
self.unpause_physics_proxy = self.create_client(
EmptySrv, self.param.topics.unpause_gazebo
)
# Start in very slow mode to ensure that everything is started before speeding up
self._update_properties(update_rate=self.param.update_rate.min)
# TODO(gz): target topics were monitored via rospy.AnyMsg subscriptions to measure
# their frequency. Recreate this with concrete message types or gz-transport once
# the sim is available.
self.subscribers = {}
super().start()
[docs]
def stop(self):
for sub in self.subscribers.values():
self.destroy_subscription(sub)
self.subscribers.clear()
if self.param.use_sync:
with contextlib.suppress(AttributeError):
self.destroy_client(self.pause_physics_proxy)
self.destroy_client(self.unpause_physics_proxy)
super().stop()
[docs]
def _calculate_update_rate(
self,
update_rate: float,
frequency: float,
desired_frequency: float,
) -> float:
"""Calculate new update rate.
Args:
update_rate: Gazebo's current maximum update_rate
frequency: Current frequency
desired_frequency: Optimal frequency
Return:
New maximum update rate.
"""
if frequency == 0:
return
# Calculate new update rate
if frequency < desired_frequency:
return max(
update_rate
- (update_rate - self.param.update_rate.min)
* self.param.update_rate.control.down
* desired_frequency
/ frequency,
self.param.update_rate.min,
)
elif frequency > desired_frequency:
return min(
update_rate
+ (self.param.update_rate.max - update_rate)
* self.param.update_rate.control.up
* desired_frequency
/ frequency,
self.param.update_rate.max,
)
[docs]
def _update_properties(self, update_rate):
# TODO(gz): read current physics properties and write the new max_update_rate via
# the gz-transport `/world/<name>/set_physics` service. Stubbed until sim is wired.
return
[docs]
def receive_sync_source(self, _):
"""Attempt to synchronize specified source topic and topic.
The basic idea is that some run time dependent components (messages) should be
published closely after another. This synchronization (pausing) ensures the same
behavior in the simulation.
"""
# TODO(gz): pause/unpause physics via gz-transport `/world/<name>/control`.
try:
self.pause_physics_proxy.call_async(EmptySrv.Request())
wait_for_message(None, self, self.param.sync.topic)
finally:
self.unpause_physics_proxy.call_async(EmptySrv.Request())
[docs]
def update(self):
"""Adjust Gazebos update rate to meet desired output frequency of the target
topic."""
# TODO(gz): the whole control loop depends on reading/writing Gazebo physics
# properties and on per-topic frequency measurements, neither of which is wired up
# for gz-sim Harmonic. This is a no-op until the gz-transport integration is added.
return
[docs]
def main(args=None):
"""Console-script entry point for the gazebo rate control node."""
rclpy.init(args=args)
try:
GazeboRateControlNode()
except KeyboardInterrupt:
pass
finally:
if rclpy.ok():
rclpy.shutdown()