Update cabot_navigation2 run_test and pedestrian plugin to improve te…

…st case implementation

* add check_metric_successful function

Signed-off-by: YumaMatsumura-kufusha <[email protected]>

* remove to call reset function from stop function

Signed-off-by: YumaMatsumura-kufusha <[email protected]>

* change to get value directly from evalator without using ROS

Signed-off-by: YumaMatsumura-kufusha <[email protected]>

* add function to wait to actor

Signed-off-by: YumaMatsumura-kufusha <[email protected]>

* update copyright

Signed-off-by: YumaMatsumura-kufusha <[email protected]>

---------

Signed-off-by: YumaMatsumura-kufusha <[email protected]>

cabot_navigation2/test/evaluator.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 ###############################################################################
-# Copyright (c) 2024  IBM Corporation
+# Copyright (c) 2024  IBM Corporation and Miraikan
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
@@ -92,7 +92,6 @@ def stop(self):
             self._evaluation_timer.cancel()
             self._evaluation_timer.destroy()
             self._evaluation_timer = None
-        self.reset()
 
     def get_evaluation_results(self):
         results = []

cabot_navigation2/test/run_test.py

@@ -134,12 +134,44 @@ def __init__(self, node, output_dir, test_module_name):
         self.result = {}
         self.test_summary = {}
         self.evaluator_summary = {}
+        self.condition_list = []
         # evaluation
         self.evaluator = None
 
     def set_evaluator(self, evaluator):
         self.evaluator = evaluator
 
+    def add_metric_condition(self, condition):
+        self.condition_list.append(condition)
+
+    def check_conditions(self, results, func):
+        if func not in self.result:
+            self.result[func] = []
+
+        for condition in self.condition_list:
+            matching_result = next((result for result in results if result["name"] == condition["name"]), None)
+
+            condition_result = {
+                "action": f"check_{condition['name']}",
+                "condition": f"{condition['condition']}"
+            }
+
+            if matching_result:
+                value = matching_result["value"]
+                condition_result["value"] = value
+
+                if eval(condition["condition"]):
+                    condition_result["success"] = True
+                    condition_result["error"] = False
+                else:
+                    condition_result["success"] = False
+                    condition_result["error"] = True
+            else:
+                condition_result["success"] = False
+                condition_result["error"] = True
+
+            self.result[func].append(condition_result)
+
     def test(self, module, test_pat, wait_ready=False):
         functions = [func for func in dir(module) if inspect.isfunction(getattr(module, func))]
 
@@ -169,8 +201,12 @@ def test(self, module, test_pat, wait_ready=False):
             logger.info(f"Testing {func}")
             self.test_func_name = func
             getattr(module, func)(self)
-            self.evaluator_summary[func] = self.evaluator.get_evaluation_results()
             self.stop_evaluation()  # automatically stop metric evaluation
+            evaluation_results = self.evaluator.get_evaluation_results()
+            self.evaluator_summary[func] = self.evaluator.get_evaluation_results()
+            self.check_conditions(evaluation_results, func)
+            self.condition_list = []
+
             success = self.print_result(self.result, func)
             self.register_action_result(func, self.result)
             self.cancel_subscription(func)
@@ -207,9 +243,15 @@ def print_result(self, result, key):
             success2 = aResult['success']
             action = aResult['action']
             if success2:
-                logger.info(f" - {action}: Success")
+                if 'condition' in aResult:
+                    logger.info(f" - {action} ({aResult['condition']}): Success")
+                else:
+                    logger.info(f" - {action}: Success")
             else:
-                logger.error(f" - {action}: Failure")
+                if 'condition' in aResult:
+                    logger.error(f" - {action} ({aResult['condition']}): Failure")
+                else:
+                    logger.error(f" - {action}: Failure")
                 logger.error(f"{aResult['error']}")
         logger.info("--------------------------")
         return success

pedestrian_plugin/pedestrian/walk_straight.py

@@ -1,4 +1,4 @@
-# Copyright (c) 2024  Carnegie Mellon University
+# Copyright (c) 2024  Carnegie Mellon University and Miraikan
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
@@ -20,13 +20,16 @@
 
 import ros
 import math
+import time
 from pedestrian import state
 
 stopped = False
+start_time = None
 
 
 def onUpdate(**args):
     global stopped
+    global start_time
 
     # parameters
     name = args['name']
@@ -37,6 +40,7 @@ def onUpdate(**args):
     stop_distance = args.get('stop_distance', 0.0)  # [m]
     radius = args.get('radius', 0.4)  # [m]
     args['radius'] = radius
+    stop_time = args.get('stop_time', 0.0)  # [s]
 
     # plugin variables
     x = args['x']
@@ -74,6 +78,13 @@ def onUpdate(**args):
     if stopped:
         vel = 0.0
 
+    # wait for movement
+    if start_time is None:
+        start_time = time.time()
+    time_diff = time.time() - start_time
+    if time_diff < stop_time:
+        vel = 0
+
     # update actor variables
     args['x'] += vel * dt * math.cos(yaw)
     args['y'] += vel * dt * math.sin(yaw)