From 20c207cec13e1857d501f89fc04f95e8f9a74b5c Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 01/16] Use scenario file MD5 hash to trigger simulation --- atos_interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/atos_interfaces b/atos_interfaces index 83ab7e720..4d9f41c9b 160000 --- a/atos_interfaces +++ b/atos_interfaces @@ -1 +1 @@ -Subproject commit 83ab7e720472f8187ac1608f2cbb2a36f4bd32de +Subproject commit 4d9f41c9b37141df5db7896de60e11f188e31a97 From 5db56ac8eac0f81c5b8e689b11735bcc8e5f9159 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 02/16] Update test scenario with custom command action --- .../resources/osc/GaragePlanScenario.xosc | 132 ++++++++++++------ 1 file changed, 93 insertions(+), 39 deletions(-) diff --git a/atos/modules/OpenScenarioGateway/tests/resources/osc/GaragePlanScenario.xosc b/atos/modules/OpenScenarioGateway/tests/resources/osc/GaragePlanScenario.xosc index 319ceb461..0c2e30a96 100644 --- a/atos/modules/OpenScenarioGateway/tests/resources/osc/GaragePlanScenario.xosc +++ b/atos/modules/OpenScenarioGateway/tests/resources/osc/GaragePlanScenario.xosc @@ -1,7 +1,6 @@ - - + @@ -65,7 +64,7 @@ - + @@ -95,9 +94,7 @@ - - - + @@ -106,7 +103,7 @@ - + @@ -115,9 +112,7 @@ - - - + @@ -125,7 +120,6 @@ - @@ -139,7 +133,6 @@ - @@ -229,7 +222,7 @@ - + @@ -261,7 +254,7 @@ - + @@ -322,9 +315,9 @@ - - - + + {"message_type": "DENM", "event_id": "ATOSEvent1", "cause_code": 12, "latitude": 0.0, "longitude": 0.0, "altitude": 0.0, "detection_time": 0} + @@ -353,7 +346,7 @@ - + @@ -371,7 +364,7 @@ - + @@ -387,17 +380,81 @@ - - + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -416,9 +473,9 @@ - + - + @@ -434,7 +491,7 @@ - + @@ -473,7 +530,7 @@ - + @@ -489,7 +546,6 @@ - @@ -499,7 +555,12 @@ - + + + + + + @@ -591,14 +652,11 @@ - - - - + @@ -640,7 +698,7 @@ - + @@ -695,7 +753,6 @@ - @@ -797,14 +854,11 @@ - - - - + @@ -846,7 +900,7 @@ - + From 4690f61287fb276f8d81d8b35c0b58db034d0002 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 03/16] Test for denm custom command action --- .../tests/test_storyboard_handler.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py index d734d41a1..1fe4fa639 100644 --- a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py @@ -30,3 +30,22 @@ def test_get_follow_trajectory_actions_to_actors_map(get_file_path): "6,start_follow_trajectory": ["6"], } assert result == expected_result + + +def test_denm_custom_command_action(get_file_path): + scenario_path = os.path.join( + os.path.dirname(__file__), "resources", "osc", "GaragePlanScenario.xosc" + ) + storyboard_handler = sh.StoryBoardHandler(scenario_path) + + # Call the method under test + result = storyboard_handler.get_custom_command_actions_map() + + # Assert the expected result + expected_result = { + "1,send_denm": { + "type": "V2X", + "content": '{"message_type": "DENM", "event_id": "ATOSEvent1", "cause_code": 12, "latitude": 0.0, "longitude": 0.0, "altitude": 0.0, "detection_time": 0}', + } + } + assert result == expected_result From 72a7da3fa5f410e305ccb0a29be4295369568a62 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 04/16] Scenario module can act on custom commands --- .../openscenariogateway.py | 51 ++++++++++++------- .../OpenScenarioGateway/storyboard_handler.py | 38 +++++++++++--- 2 files changed, 66 insertions(+), 23 deletions(-) diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 6a6236efd..9c093b71c 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -36,6 +36,7 @@ def __init__(self): self.active_objects = {} self.vehicle_catalog = None self.follow_traj_to_obj_name = {} + self.custom_command_map = {} self.scenario_file_md5hash = None @@ -87,23 +88,36 @@ 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() - and story_board_element.state == 2 - ): # 2 is a running state - - # 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: - - 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 - self.start_object_pub_.publish(start_object_msg) - break + and story_board_element.state == 2 # 2 is a running state + ): + self.handle_follow_trajectory_action(story_board_element) + elif ( + story_board_element.name in self.custom_command_map + and story_board_element.state == 2 # 2 is a running state + ): + self.handle_custom_command_action(story_board_element) + + def handle_follow_trajectory_action(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: + + 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 + self.start_object_pub_.publish(start_object_msg) + break + + def handle_custom_command_action(self, story_board_element): + self.get_logger().info("Custom command action received") + custom_command = self.custom_command_map[story_board_element.name] + if custom_command.type == "V2X": + self.get_logger().info("Sending V2X message") + # Send V2X message + self.get_logger().info(custom_command.content) def parameter_callback(self, params): for param in params: @@ -123,6 +137,9 @@ def update_scenario(self, file_name): self.follow_traj_to_obj_name = StoryBoardHandler( scenario_file ).get_follow_trajectory_actions_to_actors_map() + self.custom_command_map = StoryBoardHandler( + self.scenario_file + ).get_custom_command_actions_map() def update_active_scenario_objects(self, active_objects_name: List[str]): if len(active_objects_name) != len(set(active_objects_name)): diff --git a/atos/modules/OpenScenarioGateway/storyboard_handler.py b/atos/modules/OpenScenarioGateway/storyboard_handler.py index e6d54b62c..6089e078f 100644 --- a/atos/modules/OpenScenarioGateway/storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/storyboard_handler.py @@ -1,16 +1,24 @@ from scenariogeneration import xosc +class CustomCommandAction: + def __init__(self, type: str, content: str): + self.type = type + self.content = content + + class StoryBoardHandler: + def __init__(self, scenario_file: str): self.scenario_file = scenario_file self.xosc = xosc.ParseOpenScenario(scenario_file) - self.follow_trajectory_actions_actor_map = ( - self.collect_follow_trajectory_actions_to_actors_map() - ) + self.follow_trajectory_action_actor_map_ = {} + self.custom_command_action_map_ = {} + self.collect_action_maps() - def collect_follow_trajectory_actions_to_actors_map(self): + def collect_action_maps(self): follow_trajectory_actions = {} + custom_command_actions = {} for story in self.xosc.storyboard.stories: # Check if the element is a maneuver for act in story.acts: @@ -23,7 +31,25 @@ def collect_follow_trajectory_actions_to_actors_map(self): action.action, xosc.actions.FollowTrajectoryAction ): follow_trajectory_actions[action.name] = actors - return follow_trajectory_actions + elif ( + isinstance( + action.action, xosc.actions.UserDefinedAction + ) + and action.action.custom_command_action is not None + ): + cc_action = action.action.custom_command_action + custom_command_actions[action.name] = ( + CustomCommandAction( + type=cc_action.type, + content=cc_action.content, + ) + ) + + self.follow_trajectory_action_actor_map_ = follow_trajectory_actions + self.custom_command_action_map_ = custom_command_actions def get_follow_trajectory_actions_to_actors_map(self): - return self.follow_trajectory_actions_actor_map + return self.follow_trajectory_action_actor_map_ + + def get_custom_command_actions_map(self): + return self.custom_command_action_map_ From 243c3144ea9aeab738edf7dc0fff931c1c4e3f23 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 05/16] Move DENM handling to scenario_module --- .../EsminiAdapter/inc/esminiadapter.hpp | 2 -- .../EsminiAdapter/src/esminiadapter.cpp | 24 ------------------- .../openscenariogateway.py | 21 +++++++++++++++- 3 files changed, 20 insertions(+), 27 deletions(-) diff --git a/atos/modules/EsminiAdapter/inc/esminiadapter.hpp b/atos/modules/EsminiAdapter/inc/esminiadapter.hpp index c645c1c6d..2b1783265 100644 --- a/atos/modules/EsminiAdapter/inc/esminiadapter.hpp +++ b/atos/modules/EsminiAdapter/inc/esminiadapter.hpp @@ -80,8 +80,6 @@ class EsminiAdapter : public Module { static ATOS::Trajectory getTrajectoryFromObjectState(uint32_t,std::vector& states); static std::string projStrFromGeoReference(RM_GeoReference& geoRef); static std::map extractTrajectories(double timeStep); - static bool isSendDenmAction(const std::string& action); - static ROSChannels::V2X::message_type denmFromTestOrigin(double *llh); static void onRequestObjectTrajectory( const std::shared_ptr req, diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index 9ca470717..5327c00da 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -189,16 +189,6 @@ void EsminiAdapter::handleStartCommand() RCLCPP_INFO(me->get_logger(), "Esmini ScenarioEngine started"); } -/*! - * \brief Check if action is a DENM action. - * \param action Action name - * \return true if action is a DENM action, false otherwise -*/ -bool EsminiAdapter::isSendDenmAction(const std::string& action) -{ - return std::regex_search(action, std::regex("denm", std::regex_constants::icase)); -} - /*! * \brief Callback to be executed by esmini when story board state changes. * If story board element is an action, and the action is supported, the action is run. @@ -223,20 +213,6 @@ void EsminiAdapter::handleStoryBoardElementChange( me->storyBoardElementStateChangePub.publish(msg); } -ROSChannels::V2X::message_type EsminiAdapter::denmFromTestOrigin(double *llh) { - ROSChannels::V2X::message_type denm; - denm.message_type = "DENM"; - denm.event_id = "ATOSEvent1"; - denm.cause_code = 12; - denm.latitude = static_cast(llh[0]*10000000); // Microdegrees - denm.longitude = static_cast(llh[1]*10000000); - denm.altitude = static_cast(llh[2]*100); // Centimeters - denm.detection_time = std::chrono::duration_cast( // Time since epoch in seconds - std::chrono::system_clock::now().time_since_epoch() - ).count(); - return denm; -} - /*! * \brief Utility function to convert a ROS MONR message to Esmini representation * and report the object position to Esmini diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 9c093b71c..5f937cd2e 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import hashlib +import json +import time import atos_interfaces.srv from os import path import rclpy @@ -60,6 +62,9 @@ def __init__(self): self.start_object_pub_ = self.create_publisher( atos_interfaces.msg.ObjectTriggerStart, "start_object", 10 ) + self.v2x_message_pub_ = self.create_publisher( + atos_interfaces.msg.V2x, "v2x_message", 10 + ) # ROS services self.object_ids_pub_ = self.create_service( @@ -114,10 +119,24 @@ def handle_follow_trajectory_action(self, story_board_element): def handle_custom_command_action(self, story_board_element): self.get_logger().info("Custom command action received") custom_command = self.custom_command_map[story_board_element.name] + + if isinstance(custom_command.content, str): + custom_command.content = json.loads(custom_command.content) + if custom_command.type == "V2X": self.get_logger().info("Sending V2X message") # Send V2X message - self.get_logger().info(custom_command.content) + v2x_msgs = atos_interfaces.msg.V2x() + v2x_msgs.message_type = custom_command.content["message_type"] + v2x_msgs.event_id = custom_command.content["event_id"] + v2x_msgs.cause_code = custom_command.content["cause_code"] + v2x_msgs.detection_time = int( + time.time() + ) # Use current system time on the time stamp, maybe a user defined offset can be added in the future + v2x_msgs.altitude = int(custom_command.content["altitude"]) + v2x_msgs.latitude = int(custom_command.content["latitude"]) + v2x_msgs.longitude = int(custom_command.content["longitude"]) + self.v2x_message_pub_.publish(msg=v2x_msgs) def parameter_callback(self, params): for param in params: From 608a3fc948b392f4fcebcd7b92a623931d710603 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 06/16] Update docs --- docs/Usage/Modules/OpenScenarioGateway.md | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/docs/Usage/Modules/OpenScenarioGateway.md b/docs/Usage/Modules/OpenScenarioGateway.md index 983f38736..1e6510e50 100644 --- a/docs/Usage/Modules/OpenScenarioGateway.md +++ b/docs/Usage/Modules/OpenScenarioGateway.md @@ -5,4 +5,23 @@ This is module load and holds the chosen scenario in memory and contains various ## Features -## Usage \ No newline at end of file +*IP settings* +ScenarioModule reads the IP address for each object from the ASAM Vehicle Catalog file (located in the ~/.astazero/ATOS/Catalogs/Vehicles/VehicleCatalog directory). +The IP address must be defined as a property in the Vehicle Catalog file for each object. The property must be named `ip` and have the IP address as its value. + +´´´xml + + + +´´´ + +## Usage + +The following ROS parameters can be set for `ScenarioModule`: +```yaml +atos: + object_control: + ros__parameters: + open_scenario_file: "scenario_name.xosc" # The name of the scenario to open. Located in the ~/.astazero/ATOS/osc directory. + active_object_names: ["object1", "object2"] # List of object names to be active in the scenario. An empty list means all objects are active. +``` \ No newline at end of file From 090e7165a38eef3a44e3903f5c02eb23ca6c7e6a Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 07/16] Enable all scenario objects if override parameter is empty --- atos/modules/OpenScenarioGateway/openscenariogateway.py | 4 ++++ atos/modules/OpenScenarioGateway/storyboard_handler.py | 1 - 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 5f937cd2e..07862d816 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -165,6 +165,10 @@ def update_active_scenario_objects(self, active_objects_name: List[str]): self.get_logger().error("Active object names contain duplicates") scenario_objects = self.get_all_objects_in_scenario(self.getScenarioFilePath()) + # If the active_objects_name is empty, enable all objects + if not active_objects_name: + active_objects_name = [obj.name for obj in scenario_objects.values()] + # Remove any object that are (now) inactive for id, obj in self.active_objects.items(): if obj.name not in active_objects_name: diff --git a/atos/modules/OpenScenarioGateway/storyboard_handler.py b/atos/modules/OpenScenarioGateway/storyboard_handler.py index 6089e078f..ac50fe985 100644 --- a/atos/modules/OpenScenarioGateway/storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/storyboard_handler.py @@ -20,7 +20,6 @@ def collect_action_maps(self): follow_trajectory_actions = {} custom_command_actions = {} for story in self.xosc.storyboard.stories: - # Check if the element is a maneuver for act in story.acts: for manuever_group in act.maneuvergroup: actors = [actor.entity for actor in manuever_group.actors.actors] From 9e90c326392b01beb73294f44414f2f803766a65 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 08/16] Add custom command action ROS msg --- .../roschannels/customcommandaction.hpp | 31 +++++++++++++ atos/common/roschannels/v2xchannel.hpp | 27 ----------- .../EsminiAdapter/inc/esminiadapter.hpp | 46 +++++++++---------- .../EsminiAdapter/src/esminiadapter.cpp | 4 -- atos/modules/MQTTBridge/inc/mqttbridge.hpp | 11 +++-- atos/modules/MQTTBridge/src/mqttbridge.cpp | 40 +++++++++++----- .../openscenariogateway.py | 26 +++-------- 7 files changed, 95 insertions(+), 90 deletions(-) create mode 100644 atos/common/roschannels/customcommandaction.hpp delete mode 100644 atos/common/roschannels/v2xchannel.hpp diff --git a/atos/common/roschannels/customcommandaction.hpp b/atos/common/roschannels/customcommandaction.hpp new file mode 100644 index 000000000..5108b7d5a --- /dev/null +++ b/atos/common/roschannels/customcommandaction.hpp @@ -0,0 +1,31 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + */ +#pragma once + +#include "atos_interfaces/msg/custom_command_action.hpp" +#include "roschannel.hpp" + +namespace ROSChannels { +namespace CustomCommandAction { +const std::string topicName = "custom_command_action"; +using message_type = atos_interfaces::msg::CustomCommandAction; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node &node, const rclcpp::QoS &qos = defaultQoS) + : BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node &node, + std::function callback, + const rclcpp::QoS &qos = defaultQoS) + : BaseSub(node, topicName, callback, qos) {} +}; +} // namespace CustomCommandAction +} // namespace ROSChannels \ No newline at end of file diff --git a/atos/common/roschannels/v2xchannel.hpp b/atos/common/roschannels/v2xchannel.hpp deleted file mode 100644 index e07222bd0..000000000 --- a/atos/common/roschannels/v2xchannel.hpp +++ /dev/null @@ -1,27 +0,0 @@ -/* - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this - * file, You can obtain one at https://mozilla.org/MPL/2.0/. - */ -#pragma once - -#include "roschannel.hpp" -#include "atos_interfaces/msg/v2x.hpp" - -namespace ROSChannels { - namespace V2X { - const std::string topicName = "v2x_message"; - using message_type = atos_interfaces::msg::V2x; - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node) : - BasePub(node, topicName) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback) : BaseSub(node, topicName, callback) {} - }; - } -} \ No newline at end of file diff --git a/atos/modules/EsminiAdapter/inc/esminiadapter.hpp b/atos/modules/EsminiAdapter/inc/esminiadapter.hpp index 2b1783265..f0d90474b 100644 --- a/atos/modules/EsminiAdapter/inc/esminiadapter.hpp +++ b/atos/modules/EsminiAdapter/inc/esminiadapter.hpp @@ -5,29 +5,28 @@ */ #pragma once +#include "CRSTransformation.hpp" +#include "esmini/esminiLib.hpp" +#include "esmini/esminiRMLib.hpp" #include "module.hpp" -#include "roschannels/v2xchannel.hpp" #include "roschannels/commandchannels.hpp" -#include "roschannels/pathchannel.hpp" -#include "roschannels/monitorchannel.hpp" #include "roschannels/gnsspathchannel.hpp" -#include "roschannels/statechange.hpp" +#include "roschannels/monitorchannel.hpp" +#include "roschannels/pathchannel.hpp" #include "roschannels/scenariochannel.hpp" -#include +#include "roschannels/statechange.hpp" #include -#include "esmini/esminiLib.hpp" -#include "esmini/esminiRMLib.hpp" -#include "CRSTransformation.hpp" +#include -#include "trajectory.hpp" -#include "atos_interfaces/srv/get_test_origin.hpp" +#include "atos_interfaces/srv/get_object_ids.hpp" #include "atos_interfaces/srv/get_object_trajectory.hpp" #include "atos_interfaces/srv/get_object_trigger_start.hpp" #include "atos_interfaces/srv/get_open_scenario_file_path.hpp" -#include "atos_interfaces/srv/get_object_ids.hpp" +#include "atos_interfaces/srv/get_test_origin.hpp" +#include "trajectory.hpp" /*! - * \brief The EsminiAdapter class is a singleton class that + * \brief The EsminiAdapter class is a singleton class that * handles the communication between the esmini simulator and ATOS */ class EsminiAdapter : public Module { @@ -35,21 +34,20 @@ class EsminiAdapter : public Module { static inline std::string const moduleName = "esmini_adapter"; static inline std::filesystem::path oscFilePath; static int initializeModule(); - EsminiAdapter(EsminiAdapter const&) = delete; - EsminiAdapter& operator=(EsminiAdapter const&) = delete; + EsminiAdapter(EsminiAdapter const &) = delete; + EsminiAdapter &operator=(EsminiAdapter const &) = delete; static std::shared_ptr instance(); private: EsminiAdapter(); - ROSChannels::StartObject::Pub startObjectPub; - ROSChannels::V2X::Pub v2xPub; - ROSChannels::StoryBoardElementStateChange::Pub storyBoardElementStateChangePub; + ROSChannels::StoryBoardElementStateChange::Pub + storyBoardElementStateChangePub; ROSChannels::ConnectedObjectIds::Sub connectedObjectIdsSub; ROSChannels::Exit::Sub exitSub; ROSChannels::StateChange::Sub stateChangeSub; - std::unordered_map pathPublishers; - std::unordered_map gnssPathPublishers; + std::unordered_map pathPublishers; + std::unordered_map gnssPathPublishers; static std::unordered_map> monrSubscribers; static std::shared_ptr> objectTrajectoryService; @@ -61,7 +59,6 @@ class EsminiAdapter : public Module { rclcpp::Client::SharedPtr oscFilePathClient_; //!< Client to request the current open scenario file path rclcpp::Client::SharedPtr objectIdsClient_; //!< Client to request the ATOS object id for each openx entity name - void onMonitorMessage(const ROSChannels::Monitor::message_type::SharedPtr monr, uint32_t id); // Below is a quickfix, fix properly later static void fetchOSCFilePath(); @@ -82,12 +79,13 @@ class EsminiAdapter : public Module { static std::map extractTrajectories(double timeStep); static void onRequestObjectTrajectory( - const std::shared_ptr req, + const std::shared_ptr + req, std::shared_ptr res); - - static void onRequestTestOrigin(const std::shared_ptr, - std::shared_ptr); + static void onRequestTestOrigin( + const std::shared_ptr, + std::shared_ptr); static std::unordered_map atosIDToObjectName; static std::unordered_map objectNameToAtosId; diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index 5327c00da..98e662485 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -42,8 +42,6 @@ std::shared_ptr> EsminiAdapter::testOriginService geographic_msgs::msg::GeoPose EsminiAdapter::testOrigin = geographic_msgs::msg::GeoPose(); EsminiAdapter::EsminiAdapter() : Module(moduleName), - startObjectPub(*this), - v2xPub(*this), storyBoardElementStateChangePub(*this), connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage), exitSub(*this, &EsminiAdapter::onStaticExitMessage), @@ -103,8 +101,6 @@ std::shared_ptr EsminiAdapter::instance() { me->connectedObjectIdsSub = ROSChannels::ConnectedObjectIds::Sub(*me,&EsminiAdapter::onConnectedObjectIdsMessage); me->exitSub = ROSChannels::Exit::Sub(*me,&EsminiAdapter::onStaticExitMessage); me->stateChangeSub = ROSChannels::StateChange::Sub(*me,&EsminiAdapter::onStaticStateChangeMessage); - // Start V2X publisher - me->v2xPub = ROSChannels::V2X::Pub(*me); } return me; diff --git a/atos/modules/MQTTBridge/inc/mqttbridge.hpp b/atos/modules/MQTTBridge/inc/mqttbridge.hpp index 93ea81073..b03d001e2 100644 --- a/atos/modules/MQTTBridge/inc/mqttbridge.hpp +++ b/atos/modules/MQTTBridge/inc/mqttbridge.hpp @@ -10,8 +10,8 @@ #include "atos_interfaces/srv/new_mqtt2_ros_bridge.hpp" #include "atos_interfaces/srv/new_ros2_mqtt_bridge.hpp" #include "module.hpp" +#include "roschannels/customcommandaction.hpp" #include "roschannels/statechange.hpp" -#include "roschannels/v2xchannel.hpp" #include #include #include @@ -152,14 +152,16 @@ class MqttBridge : public Module, std::string password; std::string topic_prefix; - ROSChannels::V2X::Sub v2xMsgSub; //!< Subscriber to v2x messages requests + ROSChannels::CustomCommandAction::Sub + customCommandActionMsgSub; //!< Subscriber to v2x messages requests ROSChannels::StateChange::Sub obcStateChangeSub; //!< Subscriber to object state change requests void setupClient(); void setupMqtt2RosBridge(); void setupRos2MqttBridge(); - void onV2xMsg(const ROSChannels::V2X::message_type::SharedPtr); + void onCustomCommandActionMsg( + const ROSChannels::CustomCommandAction::message_type::SharedPtr); void onObcStateChangeMsg(const ROSChannels::StateChange::message_type::SharedPtr); @@ -167,8 +169,7 @@ class MqttBridge : public Module, void onMessage(T msg, std::string mqtt_topic, std::function convertFunc); - static json - v2xToJson(const ROSChannels::V2X::message_type::SharedPtr v2x_msg); + static json v2xToJson(const std::string v2x_msg); static json obcStateChangeToJson( const ROSChannels::StateChange::message_type::SharedPtr obc_msg); }; \ No newline at end of file diff --git a/atos/modules/MQTTBridge/src/mqttbridge.cpp b/atos/modules/MQTTBridge/src/mqttbridge.cpp index 7e0a7c398..d66d5caf3 100644 --- a/atos/modules/MQTTBridge/src/mqttbridge.cpp +++ b/atos/modules/MQTTBridge/src/mqttbridge.cpp @@ -12,7 +12,8 @@ using std::placeholders::_1; MqttBridge::MqttBridge() : Module(MqttBridge::moduleName), - v2xMsgSub(*this, std::bind(&MqttBridge::onV2xMsg, this, _1)), + customCommandActionMsgSub( + *this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)), obcStateChangeSub(*this, std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) { this->loadParameters(); @@ -380,8 +381,15 @@ void MqttBridge::newRos2MqttBridge( response->success = true; } -void MqttBridge::onV2xMsg(const V2X::message_type::SharedPtr v2x_msg) { - this->onMessage(v2x_msg, "v2x", v2xToJson); +void MqttBridge::onCustomCommandActionMsg( + const CustomCommandAction::message_type::SharedPtr + custom_command_action_msg) { + if (custom_command_action_msg->type == "v2x") { + RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic", + CustomCommandAction::topicName.c_str()); + this->onMessage(custom_command_action_msg->content, "v2x", + v2xToJson); + } } void MqttBridge::onObcStateChangeMsg( @@ -415,15 +423,25 @@ void MqttBridge::onMessage(T msg, std::string mqtt_topic, } } -json MqttBridge::v2xToJson(const V2X::message_type::SharedPtr v2x_msg) { +json MqttBridge::v2xToJson(const std::string msg_content) { + std::string modified_content = msg_content; + std::replace(modified_content.begin(), modified_content.end(), '\'', '"'); json j; - j["message_type"] = v2x_msg->message_type; - j["event_id"] = v2x_msg->event_id; - j["cause_code"] = v2x_msg->cause_code; - j["detection_time"] = v2x_msg->detection_time; - j["altitude"] = v2x_msg->altitude; - j["latitude"] = v2x_msg->latitude; - j["longitude"] = v2x_msg->longitude; + // Parse the string to a json object + try { + j = json::parse(modified_content); + } catch (json::parse_error &e) { + std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; + } + + if (j.find("message_type") == j.end()) { + std::cerr << "No message_type field in JSON object" << std::endl; + return j; + } + if (j["message_type"] == "DENM") { + j["detection_time"] = + std::chrono::system_clock::now().time_since_epoch().count(); + } return j; } diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 07862d816..67c886347 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -62,8 +62,8 @@ def __init__(self): self.start_object_pub_ = self.create_publisher( atos_interfaces.msg.ObjectTriggerStart, "start_object", 10 ) - self.v2x_message_pub_ = self.create_publisher( - atos_interfaces.msg.V2x, "v2x_message", 10 + self.custom_command_action = self.create_publisher( + atos_interfaces.msg.CustomCommandAction, "custom_command_action", 10 ) # ROS services @@ -120,23 +120,11 @@ def handle_custom_command_action(self, story_board_element): self.get_logger().info("Custom command action received") custom_command = self.custom_command_map[story_board_element.name] - if isinstance(custom_command.content, str): - custom_command.content = json.loads(custom_command.content) - - if custom_command.type == "V2X": - self.get_logger().info("Sending V2X message") - # Send V2X message - v2x_msgs = atos_interfaces.msg.V2x() - v2x_msgs.message_type = custom_command.content["message_type"] - v2x_msgs.event_id = custom_command.content["event_id"] - v2x_msgs.cause_code = custom_command.content["cause_code"] - v2x_msgs.detection_time = int( - time.time() - ) # Use current system time on the time stamp, maybe a user defined offset can be added in the future - v2x_msgs.altitude = int(custom_command.content["altitude"]) - v2x_msgs.latitude = int(custom_command.content["latitude"]) - v2x_msgs.longitude = int(custom_command.content["longitude"]) - self.v2x_message_pub_.publish(msg=v2x_msgs) + custom_command_msg = atos_interfaces.msg.CustomCommandAction() + custom_command_msg.type = custom_command.type + custom_command_msg.content = custom_command.content + + self.custom_command_action.publish(custom_command_msg) def parameter_callback(self, params): for param in params: From 6715399778c81ae4d3905f2823857cd938740206 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:13:52 +0200 Subject: [PATCH 09/16] Add RESTBridge and ICDC custom command action --- atos/CMakeLists.txt | 4 ++ atos/launch/launch_experimental.py | 9 ++- atos/modules/MQTTBridge/src/mqttbridge.cpp | 9 +-- .../openscenariogateway.py | 2 +- atos/modules/RESTBridge/CMakeLists.txt | 59 ++++++++++++++++++ atos/modules/RESTBridge/inc/restbridge.hpp | 38 ++++++++++++ atos/modules/RESTBridge/src/main.cpp | 10 +++ atos/modules/RESTBridge/src/restbridge.cpp | 62 +++++++++++++++++++ .../openscenario/garage_plan_test_scenario.py | 40 ++++++++++++ 9 files changed, 227 insertions(+), 6 deletions(-) create mode 100644 atos/modules/RESTBridge/CMakeLists.txt create mode 100644 atos/modules/RESTBridge/inc/restbridge.hpp create mode 100644 atos/modules/RESTBridge/src/main.cpp create mode 100644 atos/modules/RESTBridge/src/restbridge.cpp diff --git a/atos/CMakeLists.txt b/atos/CMakeLists.txt index 8aa50dd52..a8a652f75 100644 --- a/atos/CMakeLists.txt +++ b/atos/CMakeLists.txt @@ -64,6 +64,7 @@ set(WITH_POINTCLOUD_PUBLISHER ON CACHE BOOL "Enable PointcloudPublisher module") set(WITH_INTEGRATION_TESTING ON CACHE BOOL "Enable IntegrationTesting module") set(WITH_BACK_TO_START ON CACHE BOOL "Enable BackToStart module") set(WITH_OPEN_SCENARIO_GATEWAY ON CACHE BOOL "Enable OpenScenario Gateway module") +set(WITH_REST_BRIDGE ON CACHE BOOL "Enable RESTBridge module") set(ENABLE_TESTS ON CACHE BOOL "Enable testing on build") @@ -98,6 +99,9 @@ endif() if(WITH_BACK_TO_START) list(APPEND ENABLED_MODULES BackToStart) endif() +if(WITH_REST_BRIDGE) + list(APPEND ENABLED_MODULES RESTBridge) +endif() # Add corresponding subprojects diff --git a/atos/launch/launch_experimental.py b/atos/launch/launch_experimental.py index 58733248e..82df91f04 100644 --- a/atos/launch/launch_experimental.py +++ b/atos/launch/launch_experimental.py @@ -30,7 +30,7 @@ def get_experimental_nodes(): name="mqtt_bridge", # prefix=['gdbserver localhost:3000'], ## To use with VSC debugger parameters=[files["params"]], - arguments=["--ros-args", "--log-level", "debug"], # To get RCL_DEBUG prints + # arguments=["--ros-args", "--log-level", "debug"], # To get RCL_DEBUG prints ), Node( package="atos", @@ -46,6 +46,13 @@ def get_experimental_nodes(): name="back_to_start", parameters=[files["params"]], ), + Node( + package="atos", + namespace="atos", + executable="rest_bridge", + name="rest_bridge", + parameters=[files["params"]], + ), ] diff --git a/atos/modules/MQTTBridge/src/mqttbridge.cpp b/atos/modules/MQTTBridge/src/mqttbridge.cpp index d66d5caf3..782a0a71a 100644 --- a/atos/modules/MQTTBridge/src/mqttbridge.cpp +++ b/atos/modules/MQTTBridge/src/mqttbridge.cpp @@ -423,13 +423,14 @@ void MqttBridge::onMessage(T msg, std::string mqtt_topic, } } -json MqttBridge::v2xToJson(const std::string msg_content) { - std::string modified_content = msg_content; - std::replace(modified_content.begin(), modified_content.end(), '\'', '"'); +json MqttBridge::v2xToJson(std::string msg_content) { + std::replace(msg_content.begin(), msg_content.end(), '\'', + '"'); // Replace ROS single quotes string with double quotes to + // make it a valid JSON string json j; // Parse the string to a json object try { - j = json::parse(modified_content); + j = json::parse(msg_content); } catch (json::parse_error &e) { std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; } diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 67c886347..8c349b04d 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -145,7 +145,7 @@ def update_scenario(self, file_name): scenario_file ).get_follow_trajectory_actions_to_actors_map() self.custom_command_map = StoryBoardHandler( - self.scenario_file + self.getScenarioFilePath() ).get_custom_command_actions_map() def update_active_scenario_objects(self, active_objects_name: List[str]): diff --git a/atos/modules/RESTBridge/CMakeLists.txt b/atos/modules/RESTBridge/CMakeLists.txt new file mode 100644 index 000000000..9f03eac5e --- /dev/null +++ b/atos/modules/RESTBridge/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.8) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) + +project(rest_bridge) +find_package(atos_interfaces REQUIRED) +find_package(std_srvs REQUIRED) + +# Define target names +set(REST_BRIDGE_TARGET ${PROJECT_NAME}) + +set(COREUTILS_LIBRARY ATOSCoreUtil) +set(COMMON_LIBRARY ATOSCommon) # Common library for ATOS with e.g. Trajectory class +set(SOCKET_LIBRARY TCPUDPSocket) # Socket library for TCP/UDP communication + +get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(COREUTILS_HEADERS ${COREUTILS_LIBRARY} INCLUDE_DIRECTORIES) + +include(GNUInstallDirs) + +# Create project main executable target +add_executable(${REST_BRIDGE_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/restbridge.cpp +) +# Link project executable to common libraries +set(TARGET_LIBRARIES + ${COREUTILS_LIBRARY} + ${SOCKET_LIBRARY} + ${COMMON_LIBRARY} + ${CURL_LIBRARIES} curl) + +target_link_libraries(${REST_BRIDGE_TARGET} ${TARGET_LIBRARIES}) + +# Add include directories +target_include_directories(${REST_BRIDGE_TARGET} PUBLIC SYSTEM + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${COMMON_HEADERS} + ${COREUTILS_HEADERS} +) + +# ROS specific rules +ament_target_dependencies(${REST_BRIDGE_TARGET} + rclcpp + std_msgs + std_srvs + atos_interfaces +) + +# Installation rules +install(CODE "MESSAGE(STATUS \"Installing target ${REST_BRIDGE_TARGET}\")") +install(TARGETS ${REST_BRIDGE_TARGET} + RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) diff --git a/atos/modules/RESTBridge/inc/restbridge.hpp b/atos/modules/RESTBridge/inc/restbridge.hpp new file mode 100644 index 000000000..6e59da340 --- /dev/null +++ b/atos/modules/RESTBridge/inc/restbridge.hpp @@ -0,0 +1,38 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + */ + +#pragma once + +#include "module.hpp" +#include "roschannels/customcommandaction.hpp" +#include +#include + +using json = nlohmann::json; + +/*! + * \brief The RESTBridge is a ros2 node that demonstrates how to use the Module + * class + */ +class RESTBridge : public Module { +public: + static inline std::string const moduleName = "rest_bridge"; + RESTBridge(); + ~RESTBridge(); + +protected: + void onCustomCommandAction( + const atos_interfaces::msg::CustomCommandAction::SharedPtr msg); + +private: + ROSChannels::CustomCommandAction::Sub + customCommandActionMsgSub; //!< Subscriber to icdc messages requests + + json parseICDCCommand(std::string &msg); + void sendRESTMessages(const std::string &endpoint, const json &data); + + CURL *curl_handle; +}; diff --git a/atos/modules/RESTBridge/src/main.cpp b/atos/modules/RESTBridge/src/main.cpp new file mode 100644 index 000000000..d8168735c --- /dev/null +++ b/atos/modules/RESTBridge/src/main.cpp @@ -0,0 +1,10 @@ +#include "rclcpp/rclcpp.hpp" +#include "restbridge.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto sm = std::make_shared(); + rclcpp::spin(sm); + rclcpp::shutdown(); + return 0; +} diff --git a/atos/modules/RESTBridge/src/restbridge.cpp b/atos/modules/RESTBridge/src/restbridge.cpp new file mode 100644 index 000000000..e3cb67759 --- /dev/null +++ b/atos/modules/RESTBridge/src/restbridge.cpp @@ -0,0 +1,62 @@ +#include "restbridge.hpp" + +using namespace ROSChannels; +using namespace std::chrono_literals; +using namespace std::placeholders; + +RESTBridge::RESTBridge() + : Module(moduleName), + customCommandActionMsgSub( + *this, std::bind(&RESTBridge::onCustomCommandAction, this, _1)) { + curl_global_init(CURL_GLOBAL_ALL); + curl_handle = curl_easy_init(); +} + +RESTBridge::~RESTBridge() { + curl_easy_cleanup(curl_handle); + curl_global_cleanup(); +} + +void RESTBridge::onCustomCommandAction( + const atos_interfaces::msg::CustomCommandAction::SharedPtr msg) { + if (msg->type == "icdc") { + RCLCPP_INFO(get_logger(), "Received ICDC command: %s", + msg->content.c_str()); + json icdc = parseICDCCommand(msg->content); + sendRESTMessages(icdc["endpoint"].get(), icdc["data"]); + } +} + +json RESTBridge::parseICDCCommand(std::string &msg) { + // Parse the message and return the REST API message + std::replace(msg.begin(), msg.end(), '\'', + '\"'); // Replace single quotes with double quotes to be able to + // parse the message + json j = json::parse(msg); + return j; +} + +void RESTBridge::sendRESTMessages(const std::string &endpoint, + const json &data) { + // Send the message to the REST API + CURLcode res; + + if (curl_handle) { + curl_easy_setopt(curl_handle, CURLOPT_URL, endpoint.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, data.dump().c_str()); + + struct curl_slist *headers = NULL; + headers = curl_slist_append(headers, "accept: application/json"); + headers = curl_slist_append(headers, "Content-Type: application/json"); + curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, headers); + + // Perform the request, res will get the return code + res = curl_easy_perform(curl_handle); + + // Check for errors + if (res != CURLE_OK) { + RCLCPP_ERROR(get_logger(), "curl_easy_perform() failed: %s\n", + curl_easy_strerror(res)); + } + } +} diff --git a/scripts/openscenario/garage_plan_test_scenario.py b/scripts/openscenario/garage_plan_test_scenario.py index 3e6d5f2df..73b3f55f4 100644 --- a/scripts/openscenario/garage_plan_test_scenario.py +++ b/scripts/openscenario/garage_plan_test_scenario.py @@ -579,6 +579,46 @@ denm_osc_event.add_action(MONDEO_ID + ",send_denm", mondeo_denm_action) mondeo_maneuver.add_event(denm_osc_event) +# Mondeo object triggers ICDC action + +### ICDC Action ### +mondeo_reached_icdc_trigger_position = xosc.EntityTrigger( + name=MONDEO_ID + ",reached_camera_icdc_trigger_position", + delay=0, + conditionedge=xosc.ConditionEdge.none, + entitycondition=xosc.ReachPositionCondition( + mondeo_trigger_positions[CAMERA_DRONE_ID], 2 # Borrow the camera drone position + ), + triggerentity=MONDEO_ID, +) + +icdc_command = json.dumps( + { + "endpoint": "http://localhost:8080", + "data": { + "bandwidth": {"rate": {"unit": "mbit", "value": 1}}, + "delay": { + "correlation": {"unit": "%", "value": 10}, + "delay": {"unit": "ms", "value": 100}, + "jitter": {"unit": "ms", "value": 10}, + }, + "interface": {"name": "eth0"}, + }, + } +) +mondeo_icdc_action = xosc.UserDefinedAction( + custom_command_action=xosc.CustomCommandAction(type="icdc", content=icdc_command) +) + +icdc_osc_event = xosc.Event( + MONDEO_ID + ",high_speed_event", + xosc.Priority.parallel, +) + +icdc_osc_event.add_trigger(mondeo_reached_icdc_trigger_position) +icdc_osc_event.add_action(MONDEO_ID + ",send_icdc_command", mondeo_icdc_action) +mondeo_maneuver.add_event(icdc_osc_event) + # Mondeo triggers UFO mondeo_reached_ufo_trigger_position = xosc.EntityTrigger( From 239afbdead858963bcefa4af01b4cfd2c6a68736 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 12:58:05 +0200 Subject: [PATCH 10/16] Fix tests --- atos/CMakeLists.txt | 1 + atos/modules/OpenScenarioGateway/openscenariogateway.py | 2 +- atos/modules/OpenScenarioGateway/storyboard_handler.py | 3 +++ .../OpenScenarioGateway/tests/test_storyboard_handler.py | 9 +++++---- 4 files changed, 10 insertions(+), 5 deletions(-) diff --git a/atos/CMakeLists.txt b/atos/CMakeLists.txt index a8a652f75..0945133c7 100644 --- a/atos/CMakeLists.txt +++ b/atos/CMakeLists.txt @@ -127,6 +127,7 @@ if(BUILD_TESTING) # ament_lint_auto_find_test_dependencies() # Comment this out for now to avoid linting errors set(_pytest_tests modules/OpenScenarioGateway/tests/test_openscenariogateway.py + modules/OpenScenarioGateway/tests/test_storyboard_handler.py ) foreach(_test_path ${_pytest_tests}) get_filename_component(_test_name ${_test_path} NAME_WE) diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 8c349b04d..9db38c818 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -145,7 +145,7 @@ def update_scenario(self, file_name): scenario_file ).get_follow_trajectory_actions_to_actors_map() self.custom_command_map = StoryBoardHandler( - self.getScenarioFilePath() + self.getAbsoluteOSCPath(file_name) ).get_custom_command_actions_map() def update_active_scenario_objects(self, active_objects_name: List[str]): diff --git a/atos/modules/OpenScenarioGateway/storyboard_handler.py b/atos/modules/OpenScenarioGateway/storyboard_handler.py index ac50fe985..817afcb61 100644 --- a/atos/modules/OpenScenarioGateway/storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/storyboard_handler.py @@ -6,6 +6,9 @@ def __init__(self, type: str, content: str): self.type = type self.content = content + def __eq__(self, other): + return self.type == other.type and self.content == other.content + class StoryBoardHandler: diff --git a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py index 1fe4fa639..8d6b17844 100644 --- a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py @@ -43,9 +43,10 @@ def test_denm_custom_command_action(get_file_path): # Assert the expected result expected_result = { - "1,send_denm": { - "type": "V2X", - "content": '{"message_type": "DENM", "event_id": "ATOSEvent1", "cause_code": 12, "latitude": 0.0, "longitude": 0.0, "altitude": 0.0, "detection_time": 0}', - } + "1,send_denm": sh.CustomCommandAction( + type="V2X", + content='{"message_type": "DENM", "event_id": "ATOSEvent1", "cause_code": 12, "latitude": 0.0, "longitude": 0.0, "altitude": 0.0, "detection_time": 0}', + ) } + assert result == expected_result From 5d001fc114134711949b2162ab90c9fb581b8ae2 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 20 Aug 2024 16:00:28 +0200 Subject: [PATCH 11/16] Update docs --- atos/modules/RESTBridge/inc/restbridge.hpp | 4 +- atos/modules/RESTBridge/src/restbridge.cpp | 23 +++--- docs/Usage/Modules/OpenScenarioGateway.md | 42 +++++++++- .../openscenario/garage_plan_test_scenario.py | 79 ------------------- 4 files changed, 56 insertions(+), 92 deletions(-) diff --git a/atos/modules/RESTBridge/inc/restbridge.hpp b/atos/modules/RESTBridge/inc/restbridge.hpp index 6e59da340..7f7bfb565 100644 --- a/atos/modules/RESTBridge/inc/restbridge.hpp +++ b/atos/modules/RESTBridge/inc/restbridge.hpp @@ -31,8 +31,8 @@ class RESTBridge : public Module { ROSChannels::CustomCommandAction::Sub customCommandActionMsgSub; //!< Subscriber to icdc messages requests - json parseICDCCommand(std::string &msg); - void sendRESTMessages(const std::string &endpoint, const json &data); + json parseJsonData(std::string &msg); + void POST(const std::string &endpoint, const json &data); CURL *curl_handle; }; diff --git a/atos/modules/RESTBridge/src/restbridge.cpp b/atos/modules/RESTBridge/src/restbridge.cpp index e3cb67759..91777a248 100644 --- a/atos/modules/RESTBridge/src/restbridge.cpp +++ b/atos/modules/RESTBridge/src/restbridge.cpp @@ -19,15 +19,15 @@ RESTBridge::~RESTBridge() { void RESTBridge::onCustomCommandAction( const atos_interfaces::msg::CustomCommandAction::SharedPtr msg) { - if (msg->type == "icdc") { - RCLCPP_INFO(get_logger(), "Received ICDC command: %s", + if (msg->type == "POST_JSON") { + RCLCPP_INFO(get_logger(), "Received POST_JSON command: %s", msg->content.c_str()); - json icdc = parseICDCCommand(msg->content); - sendRESTMessages(icdc["endpoint"].get(), icdc["data"]); + json jsonData = parseJsonData(msg->content); + POST(jsonData["endpoint"].get(), jsonData["data"]); } } -json RESTBridge::parseICDCCommand(std::string &msg) { +json RESTBridge::parseJsonData(std::string &msg) { // Parse the message and return the REST API message std::replace(msg.begin(), msg.end(), '\'', '\"'); // Replace single quotes with double quotes to be able to @@ -36,18 +36,21 @@ json RESTBridge::parseICDCCommand(std::string &msg) { return j; } -void RESTBridge::sendRESTMessages(const std::string &endpoint, - const json &data) { - // Send the message to the REST API +void RESTBridge::POST(const std::string &endpoint, const json &data) { + // Send A POST request to the specified endpoint with the specified data, Use + // hardcoded headers for now CURLcode res; if (curl_handle) { + std::string json_str = data.dump(); // Store the JSON string + const char *json_data = json_str.c_str(); // Get the C-string pointer curl_easy_setopt(curl_handle, CURLOPT_URL, endpoint.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, data.dump().c_str()); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, json_data); struct curl_slist *headers = NULL; - headers = curl_slist_append(headers, "accept: application/json"); + headers = curl_slist_append(headers, "Accept: application/json"); headers = curl_slist_append(headers, "Content-Type: application/json"); + headers = curl_slist_append(headers, "charset: utf-8"); curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, headers); // Perform the request, res will get the return code diff --git a/docs/Usage/Modules/OpenScenarioGateway.md b/docs/Usage/Modules/OpenScenarioGateway.md index 1e6510e50..ff70ba63c 100644 --- a/docs/Usage/Modules/OpenScenarioGateway.md +++ b/docs/Usage/Modules/OpenScenarioGateway.md @@ -1,4 +1,4 @@ -# SampleModule +# OpenScenarioGateway ## About the module This is module load and holds the chosen scenario in memory and contains various utility functions to interact with the scenario. @@ -24,4 +24,44 @@ atos: ros__parameters: open_scenario_file: "scenario_name.xosc" # The name of the scenario to open. Located in the ~/.astazero/ATOS/osc directory. active_object_names: ["object1", "object2"] # List of object names to be active in the scenario. An empty list means all objects are active. +``` + +## Using CustomCommandActions + +The ASAM OpenX standard supports adding user defied actions to the scenario. These actions are called `CustomCommandActions`. The `OpenScenarioGateway` module provides a ROS publisher that will publish a message to the `/custom_command_action` topic when a `CustomCommandAction` is encountered in the scenario. The message will contain the type of the action and the content (details: https://releases.asam.net/OpenSCENARIO/1.0.0/Model-Documentation/content/CustomCommandAction.html) + +In the code below is an example of how you could add a `CustomCommandAction` to an Event. This code uses the scenariogeneration python library and will trigger a POST REST request with the RESTGateway module: + +```python +car_reached_trigger_position = xosc.EntityTrigger( + name="car_reached_trigger_position", + delay=0, + conditionedge=xosc.ConditionEdge.none, + entitycondition=xosc.ReachPositionCondition( + xosc.LanePosition(0, 0, 1, 3), 2 + ), + triggerentity=CAR_ID, +) + +custom_command_content = json.dumps( + { + "endpoint": "http://localhost:8080", + "data": { + "my_custom_command": "my_custom_command_data" + }, + } +) + +my_action = xosc.UserDefinedAction( + custom_command_action=xosc.CustomCommandAction(type="POST_JSON", content=custom_command_content) +) + +my_event = xosc.Event( + CAR_ID + ",high_speed_event", + xosc.Priority.parallel, +) + +my_event.add_trigger(car_reached_trigger_position) +my_event.add_action("MyCustomAction", my_action) +car_maneuver.add_event(my_event) ``` \ No newline at end of file diff --git a/scripts/openscenario/garage_plan_test_scenario.py b/scripts/openscenario/garage_plan_test_scenario.py index 73b3f55f4..2c4557663 100644 --- a/scripts/openscenario/garage_plan_test_scenario.py +++ b/scripts/openscenario/garage_plan_test_scenario.py @@ -86,9 +86,6 @@ marker_drone_brake_position = virtual_brake_position -denm_trigger_speed = 10 / 3.6 # m/s - - # Preamble init = xosc.Init() osc_params = xosc.ParameterDeclarations() @@ -544,82 +541,6 @@ camera_drone_maneuver.add_event(camera_drone_move_event) -# Mondeo object triggers DENM warning - -### DENM Action ### -mondeo_high_speed_detected = xosc.EntityTrigger( - name=MONDEO_ID + ",high_speed_detected", - delay=0, - conditionedge=xosc.ConditionEdge.none, - entitycondition=xosc.SpeedCondition(denm_trigger_speed, xosc.Rule.greaterThan), - triggerentity=MONDEO_ID, -) - -v2x_command = json.dumps( - { - "message_type": "DENM", - "event_id": "ATOSEvent1", - "cause_code": 12, - "latitude": 0.0, - "longitude": 0.0, - "altitude": 0.0, - "detection_time": 0, - } -) -mondeo_denm_action = xosc.UserDefinedAction( - custom_command_action=xosc.CustomCommandAction(type="V2X", content=v2x_command) -) - -denm_osc_event = xosc.Event( - MONDEO_ID + ",high_speed_event", - xosc.Priority.parallel, -) - -denm_osc_event.add_trigger(mondeo_high_speed_detected) -denm_osc_event.add_action(MONDEO_ID + ",send_denm", mondeo_denm_action) -mondeo_maneuver.add_event(denm_osc_event) - -# Mondeo object triggers ICDC action - -### ICDC Action ### -mondeo_reached_icdc_trigger_position = xosc.EntityTrigger( - name=MONDEO_ID + ",reached_camera_icdc_trigger_position", - delay=0, - conditionedge=xosc.ConditionEdge.none, - entitycondition=xosc.ReachPositionCondition( - mondeo_trigger_positions[CAMERA_DRONE_ID], 2 # Borrow the camera drone position - ), - triggerentity=MONDEO_ID, -) - -icdc_command = json.dumps( - { - "endpoint": "http://localhost:8080", - "data": { - "bandwidth": {"rate": {"unit": "mbit", "value": 1}}, - "delay": { - "correlation": {"unit": "%", "value": 10}, - "delay": {"unit": "ms", "value": 100}, - "jitter": {"unit": "ms", "value": 10}, - }, - "interface": {"name": "eth0"}, - }, - } -) -mondeo_icdc_action = xosc.UserDefinedAction( - custom_command_action=xosc.CustomCommandAction(type="icdc", content=icdc_command) -) - -icdc_osc_event = xosc.Event( - MONDEO_ID + ",high_speed_event", - xosc.Priority.parallel, -) - -icdc_osc_event.add_trigger(mondeo_reached_icdc_trigger_position) -icdc_osc_event.add_action(MONDEO_ID + ",send_icdc_command", mondeo_icdc_action) -mondeo_maneuver.add_event(icdc_osc_event) - - # Mondeo triggers UFO mondeo_reached_ufo_trigger_position = xosc.EntityTrigger( name=MONDEO_ID + ",reached_ufo_trigger_position", From 95590717a5450fba9b2e47e862658d0c27d012fc Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Thu, 22 Aug 2024 11:47:46 +0200 Subject: [PATCH 12/16] Update logs and formatting --- .../ObjectControl/src/states/initialized.cpp | 76 ++++++++----------- 1 file changed, 33 insertions(+), 43 deletions(-) diff --git a/atos/modules/ObjectControl/src/states/initialized.cpp b/atos/modules/ObjectControl/src/states/initialized.cpp index f126c874c..d4602088d 100644 --- a/atos/modules/ObjectControl/src/states/initialized.cpp +++ b/atos/modules/ObjectControl/src/states/initialized.cpp @@ -3,71 +3,61 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include "state.hpp" #include "journal.hpp" +#include "state.hpp" +AbstractKinematics::Initialized::Initialized() {} -AbstractKinematics::Initialized::Initialized() { - -} - -void AbstractKinematics::Initialized::connectRequest( - ObjectControl& handler) { - RCLCPP_INFO(handler.get_logger(), "Handling connect request"); - JournalRecordData(JOURNAL_RECORD_EVENT, "CONNECT received"); +void AbstractKinematics::Initialized::connectRequest(ObjectControl &handler) { + RCLCPP_INFO(handler.get_logger(), "Handling connect request"); + JournalRecordData(JOURNAL_RECORD_EVENT, "CONNECT received"); } void AbstractKinematics::Initialized::disconnectRequest( - ObjectControl& handler) { - handler.clearScenario(); + ObjectControl &handler) { + handler.clearScenario(); } - /*! ****************************************************** * \section RelativeKinematics * ****************************************************** */ -void RelativeKinematics::Initialized::connectRequest( - ObjectControl& handler) { - - if (handler.getVehicleIDs().empty()) { - RCLCPP_INFO(handler.get_logger(), "Object directory empty. Disconnecting..."); - RelativeKinematics::Initialized::disconnectRequest(handler); - } - else { - AbstractKinematics::Initialized::connectRequest(handler); - setState(handler, new RelativeKinematics::Connecting); - } +void RelativeKinematics::Initialized::connectRequest(ObjectControl &handler) { + + if (handler.getVehicleIDs().empty()) { + RCLCPP_WARN(handler.get_logger(), + "No objects are configured! Canceling connect request..."); + RelativeKinematics::Initialized::disconnectRequest(handler); + } else { + AbstractKinematics::Initialized::connectRequest(handler); + setState(handler, new RelativeKinematics::Connecting); + } } void RelativeKinematics::Initialized::disconnectRequest( - ObjectControl& handler) { - AbstractKinematics::Initialized::disconnectRequest(handler); - setState(handler, new RelativeKinematics::Idle); + ObjectControl &handler) { + AbstractKinematics::Initialized::disconnectRequest(handler); + setState(handler, new RelativeKinematics::Idle); } - /*! ****************************************************** * \section AbsoluteKinematics * ****************************************************** */ -void AbsoluteKinematics::Initialized::connectRequest( - ObjectControl& handler) { - - if (handler.getVehicleIDs().empty()) { - RCLCPP_INFO(handler.get_logger(), "Object directory empty. Disconnecting..."); - AbsoluteKinematics::Initialized::disconnectRequest(handler); - } - else { - AbstractKinematics::Initialized::connectRequest(handler); - setState(handler, new AbsoluteKinematics::Connecting); - } +void AbsoluteKinematics::Initialized::connectRequest(ObjectControl &handler) { + + if (handler.getVehicleIDs().empty()) { + RCLCPP_WARN(handler.get_logger(), + "No objects are configured! Canceling connect request..."); + AbsoluteKinematics::Initialized::disconnectRequest(handler); + } else { + AbstractKinematics::Initialized::connectRequest(handler); + setState(handler, new AbsoluteKinematics::Connecting); + } } void AbsoluteKinematics::Initialized::disconnectRequest( - ObjectControl& handler) { - AbstractKinematics::Initialized::disconnectRequest(handler); - setState(handler, new AbsoluteKinematics::Idle); + ObjectControl &handler) { + AbstractKinematics::Initialized::disconnectRequest(handler); + setState(handler, new AbsoluteKinematics::Idle); } - - From ac2c6b42a0e5e47a7db78b7bb1d418bbe68ef97b Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Thu, 22 Aug 2024 13:48:04 +0200 Subject: [PATCH 13/16] Change magic numbers to enums --- atos/modules/OpenScenarioGateway/openscenariogateway.py | 6 ++++-- atos/modules/RESTBridge/src/restbridge.cpp | 2 +- atos_interfaces | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 9db38c818..72b1a3f8d 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -21,6 +21,8 @@ SCENARIO_FILE_PARAMETER = "open_scenario_file" DEFAULT_FOLDER_PATH = path.expanduser("~/.astazero/ATOS/") +RUNNING = 2 + class ScenarioObject: def __init__(self, name: str, catalog_ref: xosc.CatalogReference): @@ -93,12 +95,12 @@ 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() - and story_board_element.state == 2 # 2 is a running state + and story_board_element.state == RUNNING ): self.handle_follow_trajectory_action(story_board_element) elif ( story_board_element.name in self.custom_command_map - and story_board_element.state == 2 # 2 is a running state + and story_board_element.state == RUNNING ): self.handle_custom_command_action(story_board_element) diff --git a/atos/modules/RESTBridge/src/restbridge.cpp b/atos/modules/RESTBridge/src/restbridge.cpp index 91777a248..5c263fb79 100644 --- a/atos/modules/RESTBridge/src/restbridge.cpp +++ b/atos/modules/RESTBridge/src/restbridge.cpp @@ -19,7 +19,7 @@ RESTBridge::~RESTBridge() { void RESTBridge::onCustomCommandAction( const atos_interfaces::msg::CustomCommandAction::SharedPtr msg) { - if (msg->type == "POST_JSON") { + if (msg->type == atos_interfaces::msg::CustomCommandAction::POST_JSON) { RCLCPP_INFO(get_logger(), "Received POST_JSON command: %s", msg->content.c_str()); json jsonData = parseJsonData(msg->content); diff --git a/atos_interfaces b/atos_interfaces index 4d9f41c9b..5956c8604 160000 --- a/atos_interfaces +++ b/atos_interfaces @@ -1 +1 @@ -Subproject commit 4d9f41c9b37141df5db7896de60e11f188e31a97 +Subproject commit 5956c8604276c69c4db508cdd10e989c18e250ee From 993799df5289c3000056166727a98fdad290e661 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Thu, 22 Aug 2024 14:31:18 +0200 Subject: [PATCH 14/16] Fix review comments --- atos/modules/RESTBridge/src/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/atos/modules/RESTBridge/src/main.cpp b/atos/modules/RESTBridge/src/main.cpp index d8168735c..1915d8cd1 100644 --- a/atos/modules/RESTBridge/src/main.cpp +++ b/atos/modules/RESTBridge/src/main.cpp @@ -3,8 +3,8 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto sm = std::make_shared(); - rclcpp::spin(sm); + auto rb = std::make_shared(); + rclcpp::spin(rb); rclcpp::shutdown(); return 0; } From d2134b879c3bcdc0bcf133ecf8c1a81e9ab596b9 Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Fri, 13 Sep 2024 16:43:40 +0200 Subject: [PATCH 15/16] Match scenario event with full path --- .../OpenScenarioGateway/openscenariogateway.py | 10 +++++++--- .../OpenScenarioGateway/storyboard_handler.py | 17 ++++++++++++++++- .../tests/test_storyboard_handler.py | 8 ++++++-- 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index 72b1a3f8d..4a235caf9 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -99,7 +99,7 @@ def story_board_element_state_change_callback(self, story_board_element): ): self.handle_follow_trajectory_action(story_board_element) elif ( - story_board_element.name in self.custom_command_map + story_board_element.full_path in self.custom_command_map and story_board_element.state == RUNNING ): self.handle_custom_command_action(story_board_element) @@ -119,8 +119,12 @@ def handle_follow_trajectory_action(self, story_board_element): break def handle_custom_command_action(self, story_board_element): - self.get_logger().info("Custom command action received") - custom_command = self.custom_command_map[story_board_element.name] + self.get_logger().info( + "Custom command action received with full_path {}".format( + story_board_element.full_path + ) + ) + custom_command = self.custom_command_map[story_board_element.full_path] custom_command_msg = atos_interfaces.msg.CustomCommandAction() custom_command_msg.type = custom_command.type diff --git a/atos/modules/OpenScenarioGateway/storyboard_handler.py b/atos/modules/OpenScenarioGateway/storyboard_handler.py index 817afcb61..4cb0f8b4f 100644 --- a/atos/modules/OpenScenarioGateway/storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/storyboard_handler.py @@ -1,3 +1,4 @@ +import sys from scenariogeneration import xosc @@ -40,7 +41,21 @@ def collect_action_maps(self): and action.action.custom_command_action is not None ): cc_action = action.action.custom_command_action - custom_command_actions[action.name] = ( + full_path = ( + story.name + + "::" + + act.name + + "::" + + manuever_group.name + + "::" + + manuever.name + + "::" + + event.name + + "::" + + action.name + ) + print(full_path, file=sys.stderr) + custom_command_actions[full_path] = ( CustomCommandAction( type=cc_action.type, content=cc_action.content, diff --git a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py index 8d6b17844..92082b7ab 100644 --- a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py @@ -43,10 +43,14 @@ def test_denm_custom_command_action(get_file_path): # Assert the expected result expected_result = { - "1,send_denm": sh.CustomCommandAction( + "story_start::start::1,maneuver_group::1,maneuver::1,high_speed_event::1,send_denm": sh.CustomCommandAction( type="V2X", content='{"message_type": "DENM", "event_id": "ATOSEvent1", "cause_code": 12, "latitude": 0.0, "longitude": 0.0, "altitude": 0.0, "detection_time": 0}', - ) + ), + "story_start::start::1,maneuver_group::1,maneuver::1,high_speed_event_2::1,send_denm_2": sh.CustomCommandAction( + type="V2X", + content='{"message_type": "DENM", "event_id": "ATOSEvent2", "cause_code": 12, "latitude": 0.0, "longitude": 0.0, "altitude": 0.0, "detection_time": 0}', + ), } assert result == expected_result From d1127cae568e59edde2f3568b5e655c62b67ed9c Mon Sep 17 00:00:00 2001 From: Robert Brenick Date: Tue, 17 Sep 2024 15:31:36 +0200 Subject: [PATCH 16/16] Refactor --- .../OpenScenarioGateway/custom_command_action.py | 7 +++++++ atos/modules/OpenScenarioGateway/storyboard_handler.py | 10 +--------- 2 files changed, 8 insertions(+), 9 deletions(-) create mode 100644 atos/modules/OpenScenarioGateway/custom_command_action.py diff --git a/atos/modules/OpenScenarioGateway/custom_command_action.py b/atos/modules/OpenScenarioGateway/custom_command_action.py new file mode 100644 index 000000000..bb1e11c5b --- /dev/null +++ b/atos/modules/OpenScenarioGateway/custom_command_action.py @@ -0,0 +1,7 @@ +class CustomCommandAction: + def __init__(self, type: str, content: str): + self.type = type + self.content = content + + def __eq__(self, other): + return self.type == other.type and self.content == other.content diff --git a/atos/modules/OpenScenarioGateway/storyboard_handler.py b/atos/modules/OpenScenarioGateway/storyboard_handler.py index 4cb0f8b4f..ca52996bd 100644 --- a/atos/modules/OpenScenarioGateway/storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/storyboard_handler.py @@ -1,14 +1,6 @@ import sys from scenariogeneration import xosc - - -class CustomCommandAction: - def __init__(self, type: str, content: str): - self.type = type - self.content = content - - def __eq__(self, other): - return self.type == other.type and self.content == other.content +from modules.OpenScenarioGateway.custom_command_action import CustomCommandAction class StoryBoardHandler: