timing stuff

bit-bots · Jan 15, 2025 · c67464d · c67464d
1 parent 0d7df46
commit c67464d
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 2 deletions.
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
@@ -16,6 +16,7 @@
 from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as parameters
 from bitbots_path_planning.planner import VisibilityPlanner
 
+from time import time
 
 class PathPlanning(Node):
     """
@@ -33,6 +34,8 @@ def __init__(self) -> None:
         self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer)
         self.controller = Controller(node=self, buffer=self.tf_buffer)
 
+        self.measurements = dict()
+
         # Subscriber
         self.create_subscription(
             PoseWithCovarianceStamped,
@@ -90,7 +93,15 @@ def step(self) -> None:
         try:
             if self.planner.active():
                 # Calculate the path to the goal pose considering the current map
+                start = time()
                 path = self.planner.step()
+                end = time()
+                if len(self.planner.robots) not in self.measurements:
+                    self.measurements[len(self.planner.robots)] = (1, end - start)
+                else:
+                    (n, avg) = self.measurements[len(self.planner.robots)]
+                    self.measurements[len(self.planner.robots)] = (n + 1, (n * avg + (end - start)) / (n + 1))
+                self.get_logger().info(f"planning steps={self.measurements}")
                 # Publish the path for visualization
                 self.path_pub.publish(path)
                 # Calculate the command velocity to follow the given path

diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
@@ -73,7 +73,6 @@ def set_robots(self, robots: sv3dm.RobotArray):
             except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
                 self.node.get_logger().warn(str(e))
         self.robots = new_buffer
-        self.node.get_logger().info(f"self.robots set to {[str(r) for r in self.robots]}")
 
     def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
         point = PointStamped()
@@ -118,7 +117,6 @@ def step(self) -> Path:
             self.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
         ).transform.translation
         start = (my_position.x, my_position.y)
-        self.node.get_logger().info(f"got dilation parameter: {self.node.config.map.inflation.dilate}")
         config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12)
         obstacles = self.robots
         if self.ball_obstacle_active and self.ball is not None: