v0.1.5

robogauge/tasks/gauge/goals/base_goal.py

@@ -8,12 +8,22 @@
 @Desc    : Base Goal Class
 '''
 
-from robogauge.tasks.simulator.sim_data import SimData
+from collections import defaultdict
+from robogauge.utils.measure import Average
 from robogauge.tasks.gauge.goal_data import GoalData
+from robogauge.tasks.simulator.sim_data import SimData
 
 class BaseGoal:
-    count = 0
-    total = 0
+    name = 'base_goal'
+
+    def __init__(self):
+        self.count = 0
+        self.total = 0
+        self.sub_name = None
+
+        self._goal_mean_metrics = defaultdict(Average)
+        self.last_sub_name = None
+        self._sub_goal_mean_metrics = defaultdict(Average)
 
     def is_done(self) -> bool:
         raise NotImplementedError
@@ -23,3 +33,29 @@ class BaseGoal:
 
     def get_goal(self, sim_data: SimData) -> GoalData:
         raise NotImplementedError
+
+    def __repr__(self):
+        if self.sub_name is None:
+            return f"{self.name}"
+        return f"{self.name}/{self.sub_name}"
+    
+    def update_metrics(self, metrics: dict):
+        """ Update step metrics for the current goal and sub-goal."""
+        if self.last_sub_name is None or self.last_sub_name != self.sub_name:
+            self.last_sub_name = self.sub_name
+            self._sub_goal_mean_metrics = defaultdict(Average)
+        for metric_name, value in metrics.items():
+            self._goal_mean_metrics[metric_name].update(value)
+            self._sub_goal_mean_metrics[metric_name].update(value)
+    
+    @property
+    def goal_mean_metrics(self):
+        """ Get the mean metrics for the current goal. """
+        return {k: float(v.mean) for k, v in self._goal_mean_metrics.items()}
+    
+    @property
+    def sub_goal_mean_metrics(self):
+        """ Get the mean metrics for the current sub-goal. """
+        if len(self._sub_goal_mean_metrics) == 0:
+            return {}
+        return {k: float(v.mean) for k, v in self._sub_goal_mean_metrics.items()}

robogauge/tasks/gauge/goals/velocity_goals.py

@@ -17,8 +17,11 @@ from robogauge.utils.helpers import class_to_dict
 from robogauge.utils.logger import logger
 
 class MaxVelocityGoal(BaseGoal):
+    name = "max_velocity"
+
     """ Goal class for maximizing velocity commands. """
     def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs):
+        super().__init__()
         kwargs.pop('enabled', None)
         if kwargs:
             logger.warning(f"Unused kwargs in MaxVelocityGoal: {kwargs}")
@@ -28,14 +31,18 @@ class MaxVelocityGoal(BaseGoal):
         self.last_reset_time = 0.0
 
         self.goals = []
-        for key, min_max in self.max_velocity.items():
-            if min_max is None: continue
-            for value in min_max:
+        for key in ['lin_vel_x', 'lin_vel_y', 'lin_vel_z', 'ang_vel_roll', 'ang_vel_pitch', 'ang_vel_yaw']:
+            if self.max_velocity.get(key) is None: continue
+            for value in self.max_velocity[key]:
                 if value != 0:
                     self.goals.append(VelocityGoal(**{key: value}))
         
         self.count = 0
         self.total = len(self.goals)
+        if len(self.goals) == 0:
+            logger.warning("MaxVelocityGoal initialized with no valid velocity commands.")
+            return
+        self.sub_name = str(self.goals[0])
 
     def is_reset(self, sim_data: SimData) -> bool:
         if sim_data.sim_time - self.last_reset_time >= self.cmd_duration:
@@ -48,10 +55,8 @@ class MaxVelocityGoal(BaseGoal):
         if self.count >= self.total:
             return None
         self.current_goal = self.goals[self.count]
+        self.sub_name = str(self.current_goal)
         return GoalData(
             goal_type='velocity',
             velocity_goal=self.current_goal
         )
-    
-    def __repr__(self):
-        return f"{self.current_goal}"