Loop logic cleanup

pyrobosim_ros/examples/demo_pddl_goal_publisher.py

@@ -24,7 +24,7 @@ def __init__(self):
             GoalSpecification, "goal_specification", 10
         )
         self.get_logger().info("Waiting for subscription")
-        while self.goalspec_pub.get_subscription_count() < 1:
+        while rclpy.ok() and self.goalspec_pub.get_subscription_count() < 1:
             time.sleep(2.0)
 
         # Create goal specifications for different examples

pyrobosim_ros/examples/demo_pddl_planner.py

@@ -51,7 +51,7 @@ def __init__(self):
             RequestWorldState, "request_world_state"
         )
         self.world_state_future_response = None
-        while not self.world_state_client.wait_for_service(timeout_sec=1.0):
+        while rclpy.ok() and not self.world_state_client.wait_for_service(timeout_sec=1.0):
             self.get_logger().info("Waiting for world state server...")
 
         # Create the world and planner
@@ -147,6 +147,7 @@ def main():
     rclpy.init()
     planner_node = PlannerNode()
 
+    rate = planner_node.create_rate(10)
     while rclpy.ok():
         if (not planner_node.planning) and planner_node.latest_goal:
             planner_node.request_world_state()
@@ -158,6 +159,7 @@ def main():
             planner_node.do_plan()
 
         rclpy.spin_once(planner_node)
+        rate.sleep()
 
     planner_node.destroy_node()
     rclpy.shutdown()