From b2d31ae927fbff073f66982932795239d9a5c7e1 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 8 Oct 2024 13:18:23 +0200 Subject: [PATCH 1/3] Start on more xosc actions --- .../OpenScenarioGateway/storyboard_handler.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/atos/modules/OpenScenarioGateway/storyboard_handler.py b/atos/modules/OpenScenarioGateway/storyboard_handler.py index ca52996bd..116c55a84 100644 --- a/atos/modules/OpenScenarioGateway/storyboard_handler.py +++ b/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 ( From 058e1b5720f3e5af4b7b7b855ec2ddbc6d6186a4 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 15 Oct 2024 14:59:31 +0200 Subject: [PATCH 2/3] Update test --- .../tests/test_storyboard_handler.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py index 92082b7ab..7ff44f70b 100644 --- a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py +++ b/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 From 9b1183a12343409ec80eca83660ee38e469de5c1 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 15 Oct 2024 15:00:05 +0200 Subject: [PATCH 3/3] Only start an object once per scenario --- .../openscenariogateway.py | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index f1af7145a..560f725ce 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/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,22 +95,23 @@ 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}" ) @@ -117,6 +119,8 @@ def handle_follow_trajectory_action(self, story_board_element): 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)