Start on more xosc actions

atos/modules/OpenScenarioGateway/openscenariogateway.py

@@ -29,6 +29,7 @@ def __init__(self, name: str, catalog_ref: xosc.CatalogReference):
         self.name = name
         self.catalog_ref: xosc.CatalogReference = catalog_ref
         self.ip: str = None
+        self.started: bool = False
 
 
 class OpenScenarioGateway(Node):
@@ -39,7 +40,7 @@ def __init__(self):
         # Class variables
         self.active_objects = {}
         self.vehicle_catalog = None
-        self.follow_traj_to_obj_name = {}
+        self.start_actions_to_obj_name = {}
         self.custom_command_map = {}
 
         self.scenario_file_md5hash = None
@@ -94,29 +95,32 @@ def init_callback(self, msg):
 
     def story_board_element_state_change_callback(self, story_board_element):
         if (
-            story_board_element.name in self.follow_traj_to_obj_name.keys()
+            story_board_element.name in self.start_actions_to_obj_name.keys()
             and story_board_element.state == RUNNING
         ):
-            self.handle_follow_trajectory_action(story_board_element)
+            self.handle_start_actions(story_board_element)
         elif (
             story_board_element.full_path in self.custom_command_map
             and story_board_element.state == RUNNING
         ):
             self.handle_custom_command_action(story_board_element)
 
-    def handle_follow_trajectory_action(self, story_board_element):
+    def handle_start_actions(self, story_board_element):
         # Iterate through active objects to send the start command for the target objects
         for object_id, object in self.active_objects.items():
-            target_object_name = self.follow_traj_to_obj_name[story_board_element.name]
-            if object.name in target_object_name:
-
+            target_object_name = self.start_actions_to_obj_name[
+                story_board_element.name
+            ]
+            if object.name in target_object_name and not object.started:
                 self.get_logger().info(
                     f"Starting object {object.name} with id {object_id}"
                 )
                 start_object_msg = atos_interfaces.msg.ObjectTriggerStart()
                 start_object_msg.id = object_id
                 start_object_msg.stamp = self.get_clock().now().to_msg()
                 self.start_object_pub_.publish(start_object_msg)
+
+                object.started = True
                 break
 
     def handle_custom_command_action(self, story_board_element):
@@ -148,7 +152,7 @@ def update_scenario(self, file_name):
             self.get_logger().error("File does not exist: {}".format(scenario_file))
             return
         self.get_logger().info("Loading scenario file: {}".format(scenario_file))
-        self.follow_traj_to_obj_name = StoryBoardHandler(
+        self.start_actions_to_obj_name = StoryBoardHandler(
             scenario_file
         ).get_follow_trajectory_actions_to_actors_map()
         self.custom_command_map = StoryBoardHandler(
@@ -181,6 +185,8 @@ def update_active_scenario_objects(self, active_objects_name: List[str]):
                         obj.name, id
                     )
                 )
+                # Reset the started flag
+                obj.started = False
 
     def get_all_objects_in_scenario(self, scenario_file) -> List[ScenarioObject]:
         scenario = xosc.ParseOpenScenario(scenario_file)

atos/modules/OpenScenarioGateway/storyboard_handler.py

@@ -22,8 +22,17 @@ def collect_action_maps(self):
                     for manuever in manuever_group.maneuvers:
                         for event in manuever.events:
                             for action in event.action:
-                                if isinstance(
-                                    action.action, xosc.actions.FollowTrajectoryAction
+                                if (
+                                    isinstance(
+                                        action.action,
+                                        xosc.actions.FollowTrajectoryAction,
+                                    )
+                                    or isinstance(
+                                        action.action, xosc.actions.AbsoluteSpeedAction
+                                    )
+                                    or isinstance(
+                                        action.action, xosc.actions.RelativeSpeedAction
+                                    )
                                 ):
                                     follow_trajectory_actions[action.name] = actors
                                 elif (

atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py

@@ -11,7 +11,7 @@ def test_init_storyboard_handler(get_file_path):
     assert storyboard_handler
 
 
-def test_get_follow_trajectory_actions_to_actors_map(get_file_path):
+def test_get_start_actions_to_actors_map(get_file_path):
     scenario_path = os.path.join(
         os.path.dirname(__file__), "resources", "osc", "GaragePlanScenario.xosc"
     )
@@ -23,11 +23,24 @@ def test_get_follow_trajectory_actions_to_actors_map(get_file_path):
     # Assert the expected result
     expected_result = {
         "1,init_follow_trajectory": ["1"],
+        "1,slow_down": ["1"],
+        "1,mondeo_accelerate": ["1"],
+        "1,brake_to_stop": ["1"],
+        "5,brake_to_stop": ["5"],
+        "5,start_follow_trajectory": ["5"],
+        "5,set_speed": ["5"],
+        "2,brake_to_stop": ["2"],
         "2,start_follow_trajectory": ["2"],
-        "3,start_follow_trajectory": ["3"],
+        "2,set_speed": ["2"],
+        "4,brake_to_stop": ["4"],
         "4,start_follow_trajectory": ["4"],
-        "5,start_follow_trajectory": ["5"],
+        "4,set_speed": ["4"],
+        "3,brake_to_stop": ["3"],
+        "3,start_follow_trajectory": ["3"],
+        "3,set_speed": ["3"],
+        "6,brake_to_stop": ["6"],
         "6,start_follow_trajectory": ["6"],
+        "6,set_speed": ["6"],
     }
     assert result == expected_result